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1  Abstract 

Unmanned  vehicles  have  established  an  important  place  in  the  modern  battlefield.  They 
play  a  key  role  in  intelligence  and  surveillance  while  not  putting  human  lives  in  harm’s  way. 

The  United  States  Navy  recognizes  this  and  has  created  The  Navy  Unmanned  Surface  Vessel 
(USV)  Master  Plan ,  which  lists  autonomous  launch  and  recovery  (L&R)  as  a  key  challenge. 

A  necessary  enabling  technology  is  the  ability  to  automatically  identify  L&R  sites  from 
sensor  data.  This  project  focuses  on  the  identification  of  suitable  docking  sites  from  three 
dimensional  Light  Detection  and  Ranging  (LiDAR)  scans.  A  LiDAR  sensor  is  a  sensor  that 
collects  range  images  from  a  rotating  array  of  vertically  aligned  lasers. 

Our  solution  leverages  open  source  C++  code  from  Point  Cloud  Library  —  “a  standalone, 
large  scale,  open  project  for  2D/3D  image  and  point  cloud  processing.”  Given  a  LiDAR  point 
cloud  our  identification  algorithm  proceeds  as  follows.  First  the  Random  Sample  Consensus 
(RANSAC)  algorithm  is  used  to  isolate  horizontal  planar  surfaces  that  may  belong  to  the  dock. 
After  removing  planar  dock  points,  Euclidean  Cluster  Recognition  is  used  to  isolate  point 
clusters  that  are  potential  vertical  pilings.  Bayes’  Theorem  is  used  to  compute  the  probability 
that  each  cluster  matches  the  characteristics  of  piling.  For  each  candidate  piling,  the  origin  of 
that  cluster  is  compared  to  the  location  of  the  target  dock’s  planar  surface.  The  dock  can  be 
identified  by  the  relation  of  the  pilings  location  to  the  dock’s  planar  surface.  The  final  output  of 
the  algorithm  will  be  a  sub-set  of  points,  isolated  from  the  original  cloud,  that  are  hypothesized 
to  correspond  to  the  dock. 

Keywords:  Algorithm,  Dock,  Locations,  Point  Clouds,  LiDAR,  Identify 
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4  Background  Information 

4.1  Motivation 

The  strategic  vision  for  the  Navy  and  the  Department  of  Defense  includes  an  increased 
reliance  on  unmanned  systems.  Unmanned  systems  have  demonstrated  the  ability  to  reduce  cost 
because  they  are  cheaper  to  build  and  maintain  than  manned  systems.  They  also  have  the 
capacity  to  maintain  persistent  awareness  for  Intelligence,  Surveillance,  and  Reconnaissance 
(ISR)  missions,  providing  information  on  long-term  trends  about  a  target  that  would  otherwise 
require  numerous  manned  vehicles.  This  allows  the  commander  to  focus  manpower  on 
immediate  threats.  The  benefits  of  unmanned  systems  have  been  outlined  in  The  Navy 
Unmanned  Surface  Vessel  (USV)  Master  Plan.  In  this  plan,  the  systems  are  described  as  manual 
(manned),  semi-autonomous  (unmanned  but  with  user  inputs),  or  autonomous  (completely 
unmanned). 

One  of  the  engineering  challenges  presented  by  the  USV  Master  Plan  is  the  Launch  and 
Recovery  (L&R)  of  the  semi-autonomous  and  autonomous  surface  vessels.  Whether  it  is  from  a 
stationary  dock  or  another  surface  ship  at  sea,  the  United  States  Navy  and  the  Department  of 
Defense  want  unmanned  surface  vessels  to  be  able  to  launch  and  recover  on  their  own.  For  an 
unmanned  surface  vessel  to  recover  itself  after  a  mission,  it  must  possess  the  fundamental 
capability  of  “dock  recognition.” 

4.2  Sensor  Information 

The  sensor  used  in  this  project  to  identify  suitable  dock  locations  is  a  three-dimensional 
Light  Detection  and  Ranging  (LiDAR)  sensor.  This  sensor  is  an  array  of  lasers  that  spins  in  a 
360°  horizontal  field  of  view  and  collects  range  measurements  of  this  surrounding  area,  see 
Figure  1  for  an  illustration  of  a  LiDAR  system.  The  sensor  receives  reflected  energy  from  the 
lasers  in  order  to  calculate  the  range  measurements.  From  these  range  measurements  of  the 
surrounding  area,  the  data  is  then  converted  into  Cartesian  coordinates  to  be  viewed  as  a  three 
dimensional  scene,  referred  to  as  a  point  cloud.  Three-dimensional  (3-D)  LiDARs  have  proved 
themselves  very  useful  on  many  autonomous  ground  vehicles,  such  as  the  Google  Driverless  Car 
Project,  the  DARPA,  Defense  Advanced  Research  Projects  Agency,  Grand  Challenge,  and  the 
DARPA  Urban  Challenge  (Bohren  et  al.  2008;  Leonard  et  al.  2009).  One  last  deciding  factor  on 
why  the  three  dimensional  LiDAR  was  chosen  was  for  the  fact  that  most  autonomous  vehicles 
are  already  using  the  sensor  The  algorithm  will  be  taking  data  from  a  sensor  that  will  already  be 
on  the  autonomous  surface  vessel. 
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Ground 


Figure  1 :  Illustration  representing  a  LiDAR  Sensor 


LiDARs  can  be  compared  to  Radio  Detection  and  Ranging  (RADAR)  systems  in  the 
sense  that  both  propagate  energy  and  measure  range  to  detect  objects.  The  difference  is  that  the 
propagated  energy  from  LiDARs  is  from  lasers  while  RADARs  utilize  radio  waves;  wavelengths 
propagating  from  lasers  are  short  compared  to  radio  waves.  Short  wavelengths  imply  a  few 
differences;  namely:  (1)  that  LiDARs  will  have  shorter  range  and  higher  scattering  than 
RADAR;  and  (2)  that  very  narrow  beams  can  be  formed,  and  with  these  narrow  beams,  comes 
higher  resolution.  The  return  that  the  LiDAR  generates  is  a  high-resolution  reflection  of  the 
surrounding  area.  In  Figure  2,  the  colors  reference  which  lasers  each  range  measurement  is 
measured  from.  The  red  is  measured  by  the  lasers  that  are  lower  in  the  laser  array,  while  the  blue 
is  measured  from  the  lasers  on  the  top  of  the  array.  This  reflection  can  be  used  in  mobile 
autonomous  robots  by  programs  that  interpret  the  raw  data  provided  by  the  LiDAR  and  then 
controlthe  robot  accordingly. 
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Figure  2:  A  LiDAR  Scan  of  a  pier  with  boats 
(Maritime  2009) 

Three  dimensional  LiDARs  have  the  capability  to  measure  ranges  at  high  resolution.  These 
two  abilities  combined,  with  the  fact  that  using  a  3-D  LiDAR  will  not  require  any  alterations  to  a 
dock,  set  it  apart  from  any  other  possible  sensors  for  this  project.  A  project  objective  was  to  not 
modify  the  target  docking  locations  because  if  the  algorithm  was  dependent  on  a  modified  dock, 
the  number  of  possible  suitable  docking  locations  would  decrease.  Figure  3  compares  the  3-D 
LiDAR,  to  other  sensors  that  could  possibly  have  been  used  for  this  project;  notice  how  the  3-D 
LiDAR  matches  the  desired  properties. 


Sensors 

High  Retention 

Detect  Range 

Does  Not 
Require  Dock 
Modification 

3-D  LiDAR 

Yes 

Yes 

Yes 

RADAR 

No 

Yes 

Yes 

Cameras 

Yes 

No 

No 

Ultrasonic 

No 

Yes 

Yes 

Beacons/Transponders 

No 

Yes 

No 

2-D  LiDAR 

No 

Yes 

Yes 

Figure  3:  A  table  listing  the  possible  sensors  to  use  to  identify  a  dock 

The  LiDAR  that  will  be  used  for  this  project  will  be  the  Velodyne  High  Definition  LiDAR  - 
32E,  or  the  HDL-32E  (Figure  4).  The  HDL-32E  uses  32  lasers  aligned  from  +10.67°  to  negative 
30.67°in  a  vertical  field  of  view,  FOV.  The  HDL-32E  uses  a  rotating  head  that  gives  it  a  360° 
FOV  horizontally.  This  allows  the  HDL-  32E  to  collect  up  700,000  range  measurements  points  a 
second  that  represents  the  three  dimensional  surrounding  environment.  The  HDL-32E’s  lasers 
are  emitted  at  a  wavelength  of  905  nanometers;  the  laser  is  Class  1  (Velodyne  201 1).  It  provides 
range  measurements  up  to  one  hundred  meters  with  an  error  of  +/-  two  centimeters  while  rotating 
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at  ten  Hertz.  The  Velodyne  LiDAR  also  has  an  inertial 
measurement  unit  (IMU)  containing  accelerometers 
and  gyros,  by  which  it  can  estimate  its  own  orientation. 
This  can  be  used  to  correct  scans  taken  on  a  platform 
that  experiences  pitch,  roll,  yaw,  and  heave. 

The  HDL-32E,  and  other  LiDARs,  share  some 
limitations.  Since  the  sensor  is  a  range  finder,  it  can 
only  detect  the  first  object  that  it  hits.  Everything 
behind  that  initial  data  point  is  unknown.  This  creates 
a  shadow  area  behind  the  close  objects.  Another 
shadow  area  is  created  from  the  sensor’s  vertical  field 
of  view.  The  sensor  cannot  hit  the  objects  beneath  it 
because  the  bottom  laser  in  the  array  does  not  aim 
straight  down.  This  produces  a  circular  shadow  area 
directly  beneath  the  LiDAR;  refer  back  to  Figure  1  for 
an  image  depicting  the  shadow  areas.  Also,  the  laser 
returns  from  very  reflective  objects  are  often  inaccurate 
and  can  cause  discrepancies  in  the  raw  data.  In 
Figure  4:  Velodyne  HDL-32E  LiDAR  addition,  transparent  objects  often  produce  no  return. 

This  can  be  seen  back  in  Figure  2  where  the  water  is 
not  detected  around  the  pier.  This  is  because  the  transparency  of  the  water  diffracts  the  laser 
beams  causing  no  return  instead  of  reflecting  the  energy  back  towards  the  sensor  like  a  normal 
solid  object  (Maritime  2009).  This  means  that  when  the  HDL-32E  is  placed  on  a  boat,  the  water 
should  produce  no  return.  This  would  not  be  a  problem  since  water  is  not  considered  an  object; 
however,  the  LiDAR  sensor  picks  up  white  caps  and  surface  debris. 


5  Experimental  Details 

5.1  Point  Cloud  Library  (PCL) 

The  decision  was  made  to  use  as  many  pre-existing  algorithms  as  possible  in  order  to  save 
time  and  effort  on  this  project.  At  the  beginning  of  the  academic  year,  we  made  a  decision  to  use 
Point  Cloud  Library,  or  PCL.  Point  Cloud  Library  is  a  standalone,  large  scale,  open  project  for 
2D/3D  image  and  point  cloud  processing.  PCL  provides  a  library  of  pre- written  C++  functions 
for  manipulating  point  cloud  data,  or  PCD.  Point  cloud  data  files  (.pcd)  are  used  to  store  the  data 
in  this  project;  just  as  a  picture  file  may  be  stored  as  a  “.jpeg”.  Besides  interacting  well  with 
PCD  files,  PCL  offered  many  other  capabilities  that  were  deemed  important  in  this  project. 

Some  examples  are  filtering  data,  registering  data  sets,  and  identifying  key  points.  In  the  original 
project  outline,  the  planar  surface  of  the  dock  was  to  be  identified  using  robust  estimation 
algorithms,  like  Random  Sample  Consensus  (RANSAC).  The  fact  that  PCL  had  a  library  that 
already  possessed  these  capabilities  made  it  an  advantageous  tool  for  use.  Another  benefit  that 
comes  from  using  an  open-source  library  like  PCL  is  that  others  may  freely  use  the  code  in  order 
update  this  algorithm,  or  use  this  algorithm  to  inspire  their  own  ideas. 
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5.2  OpenCV 

The  final  algorithm  also  uses  image  processing  techniques  to  isolate  the  features  of  the 
desired  docking  locations.  In  order  to  gain  the  benefits  of  using  computer  vision  techniques 
while  not  losing  time  by  writing  algorithms,  it  was  decided  that  OpenCV  would  be  used  for  the 
image  processing  steps  just  as  PCL  was  used  for  the  point  cloud  portions.  OpenCV  is  an  open 
source  computer  vision  library  written  in  C  and  C++  and  works  on  many  platforms  and  many 
specific  coding  languages  (Bradski  and  Kaehler  2008).  OpenCV  provides  simple-to-use 
algorithms  for  the  image  processing  portion  of  this  algorithm.  The  images  that  these  image 
processing  techniques  will  be  used  on  are  created  in  the  algorithm  from  point  cloud  data  and  are 
from  other  sensors. 

5.3  Data  Collection 
5.3.1  Target  Dock 

There  are  many  types  of  dock  designs.  In  order  to  make  this  project  tractable  it  was 
determined  that  only  one  specific  style  of  dock  need  to  be  identified.  The  targeted  dock  in  the 
project  has  a  raised  planar  surface  surrounded  by  pilings.  This  is  important  because  both  level 
surfaces  and  pilings  provide  identifiable  features  in  a  LiDAR  scan.  The  dock  is  identified  using 
the  pattern  from  the  spacing  between  the  identified  pilings  in  relation  to  the  planar  surface  of  the 
dock.  The  reason  a  flat  dock  with  no  pilings  is  not  a  targeted  docking  site  in  this  algorithm  is 
because  it  lacks  distinguishing  geometric  features.  A  parked  barge  or  seawall  could  appear  flat 
enough  to  register  as  a  dock  which  is  undesirable.  The  pilings  are  used  to  further  differentiate 
the  desired  docking  locations  from  other  objects  in  the  water. 

For  this  specific  project,  it  was  established  that  most  of  the  testing  would  be  done  off  one 
dock.  This  target  dock  used  is  located  on  the  northern  side  of  Dewey  Field  behind  Rickover  Hall 
on  the  United  States  Naval  Academy  campus,  see  Figure  5. 


Figure  5 :  Aerial  view  of  the  target  dock  (center) 

Imagery  ©2013  U.S.  Geological  Survey.  Map  data  ©2013  Google 
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5.3.2  Data  Collecting  Process 


In  order  to  collect  data,  the  LiDAR  would  be  placed  on  a  cart  and  a  MATLAB  program 
was  used  to  collect  data  of  one  scan,  or  one  360  degree  rotation,  of  the  LiDAR.  These  scans 
were  saved  as  MATLAB  figures  for  visual  reference  and  as  PCD  files  to  be  imported  into  PCL. 
The  scans  were  collected  from  stationary  positions.  The  decision  was  made  to  collect  data  using 
a  cart  instead  of  a  boat  because  the  cart  allowed  for  the  elimination  of  many  variables,  like  the 
motion  of  the  waves,  along  with  the  elimination  of  risk  to  the  equipment.  Along  with  the  PCD 
files,  pictures  were  taken  of  the  surrounding  area  for  use  as  reference.  Lastly  the  scan  location 
was  referenced  on  an  overhead  shot  of  the  area;  please  reference  Figure  6  to  see  the  location  of 
each  scan  and  Figure  7  for  an  example  of  the  point  cloud  data  collected  and  corresponding 
photos. 
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Figure  6:  Locations  of  nineteen  data  scans 
Imagery  ©2013  U.S.  Geological  Survey.  Map  data  ©2013  Google 
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Figure  7:  Top  image  is  a  LiDAR  scan  of  position  one  from  Figure  6  and  the  bottom  half  is 
four  photos  that  correspond  with  the  LiDAR  scan 
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Initially  when  data  was  collected,  the  LiDAR  was  about  five  feet  off  the  ground.  The 
point  clouds  that  were  collected  at  this  height  were  deemed  unusable  because  the  majority  of  the 
points  collected  corresponded  to  the  sidewalk  within  two  meters  from  the  sensor.  Very  few 
points  corresponded  to  the  dock  or  other  features  commonly  seen  in  a  maritime  environment. 
This  was  due  to  the  LiDAR’s  field  of  view,  or  FOV.  As  stated  earlier,  the  LiDAR’s  vertical 
FOV  is  mostly  negative,  meaning  that  most  of  the  lasers  are  pointed  down.  The  decision  was 
made  to  raise  the  LiDAR  off  the  ground  because  this  would  result  in  a  dataset  where  the  data 
points  were  located  farther  from  the  sensor.  Figure  8  shows  data  collected  at  the  different 
heights  from  the  same  location.  It  can  be  seen  that  the  dataset  collected  at  the  higher  altitude 
produces  a  larger  scan.  It  also  more  accurately  mimics  the  height  in  which  the  LiDAR  would  sit 
on  an  autonomous  surface  vessel  if  it  were  to  be  used  in  this  application;  typically,  the  height  of 
a  mast  or  where  a  RADAR  is  placed  on  a  surface  vessel  is  nearly  twenty  feet  above  sea  level. 
The  new  height  that  the  LiDAR  was  set  to  was  about  15  feet.  Figure  9  shows  the  cart  with  the 
sensor  on  a  fifteen  foot  mast,  while  Figure  10  shows  the  cart  on  a  dock  in  the  process  of  data 
collection.  Scans  were  then  collected  with  the  raised  LiDAR  and  this  produced  more  usable 
results,  meaning  that  more  returns  corresponding  to  features  over  five  meters  away  were 
collected. 
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Figure  8:  The  top  scan  is  from  an  elevated  height  of  five  feet,  while  the  bottom  scan  is  from  an 
elevated  height  of  15  feet  and  contains  more  information  about  the  surroundings 


Figure  9:  Data  collection  cart  with  the  sensor  elevated  15  feet. 
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Figure  10:  Image  of  the  cart  during  the  process  of  collecting  data. 

These  scans  that  include  objects  at  longer  ranges  are  more  desirable;  however,  the 
additional  scans  from  the  new  height  produced  a  new  problem.  The  resolution  of  the  dataset  was 
reduced  in  the  dock  region.  This  is  due  to  the  fact  that  the  linear  distance  between  adjacent  laser 
readings  increases  as  objects  are  farther  away  from  the  sensor.  This  can  be  seen  back  in  Figure  8. 
Note  that  the  center  black  circle  is  the  shadow  area  mentioned  previously.  The  distance  between 
adjacent  laser  scans  at  twenty  five  meters  away  from  the  sensor  is  nearly  four  meters.  This  leaves 
gaps  in  the  point  clouds  in  which  no  data  is  collected.  This  is  a  problem  because  the  proposed 
algorithm  relies  on  identifying  pilings,  but  pilings  will  not  be  identified  if  they  fall  into  the  data 
gaps  caused  by  the  divergence  of  the  lasers.  This  leaves  the  task  of  somehow  creating  a  higher 
resolution  data  set  that  will  not  miss  any  features  in  the  dock. 
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6  Method 

This  algorithm  is  a  multi-step  process  that  utilizes  both  the  3-D  features  of  the  point  clouds 
and  the  2-D  arrangement  of  the  data  points  as  seen  from  an  overhead  view.  A  quick  overview  of 
the  method  can  be  seen  in  Figure  1 1  below. 


Dock  Isolation  Method  Psuedocode 

1 .  Acquire  a  Scan 

2.  Register  the  Scan 

a.  SAC-LA  Alignment 

b.  ICP  Alignment 

3.  Remove  the  Shore 

4.  Find  the  Planar  Surface  of  the  Dock 

5.  Identify  Pilings 

a.  Extract  the  Plane 

b.  Use  Probability  to  Identify  Pilings 

6.  Compare  Identified  Pilings  with  Objects  Removed  from 
the  Shore 

7.  Evaluate  Identified  Point  Clouds 

Figure  1 1 :  Dock  isolation  Method  Psuedocode 

6. 1  Registration  and  Concatenation  of  Multiple  Point  Clouds 

6.1.1  Registration 

Raising  the  LiDAR  sensor  higher  off  the  ground  created  scans  that  were  larger  in  size 
since  the  lasers  could  then  detect  objects  farther  away.  However,  this  compromised  the 
resolution  for  each  scan.  Purchasing  a  higher  resolution  LiDAR  was  not  an  option.  Also,  taking 
scans  from  closer  ranges  so  that  the  LiDAR  would  be  able  to  identify  more  of  the  dock’s  features 
was  undesirable  because  it  eliminated  the  applicability  this  algorithm  would  have  to  autonomous 
surface  vessels.  Instead,  the  decision  was  made  to  register,  or  align,  a  series  of  scans,  taken  from 
different  positions,  into  a  single  higher  resolution  point  cloud.  Figure  12  shows  two  different 
scans  taken  at  two  different  positions  three  meters  apart. 
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Figure  12:  Two  point  clouds  taken  three  meters  apart. 
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Registration  uses  different  mathematical  optimization  techniques  to  align  the  two 
different  data  clouds.  The  output  of  a  standard  registration  function  a  homogeneous 
transformation  matrix  (M-+1)  that  aligns  point  cloud  i  to  point  cloud  i+1.  A  homogeneous 
transformation  matrix  is  a  mathematical  representation  of  a  rotation  and  translation  between  two 
reference  frames,  seen  here: 


-Rn 

^21 

R31 

T4i 

R\2 

^22 

R32 

T42 

Rl3 

^23 

R33 

T43 

.  0 

0 

0 

1 

The  top  left  three  by  three  matrix  represents  one  coordinate  system’s  rotation  in  relation 
to  another  coordinate  system.  For  example,  [Rn  R12  R13Y  represents  the  x  axis  of  frame  i 
expressed  in  terms  of  unit  vectors  (i.e.  x,  y,  and  z  axes)  of  frame  i+1.  The  three  by  one  vector,  T 
in  Equation  1,  represents  the  translation  of  frame,  between  the  origin  of  frame  i  and  the  origin  of 
frame  i+1,  expressed  in  terms  of  unit  vectors.  The  bottom  row  is  always  three  zeros  and  a  one. 
The  transformation  matrix  contains  all  the  information  that  is  needed  to  transform,  or  align,  a 
point  cloud  to  another  point  cloud.  The  other  benefit  of  using  transformation  matrices  is  that 
multiple  coordinate  systems  can  be  aligned  through  matrix  multiplication.  A  quick  example 
being  if  there  are  two  transformation  matrices,  and  one  relating  coordinate  system  “i”  to 
coordinate  system  “i+1”,  and  the  other  relating  coordinate  system  “i+1”  to  coordinate  system 
“i+2”.  One  can  quickly  find  the  transformation  from  “i”  to  “i+2”  by  multiplying  the  two 
transformation  matrices  together; 


M\+2  = 

PCL  provides  a  library  of  registration  functions  that  offers  multiple  ways  to  try  to  register 
different  point  clouds  ( i.e.  estimate  M\+1 ). 

6. 1.1.1  Sample  Consensus-  Initial  Alignment 

Sample  Consensus-Initial  Alignment,  or  SAC-IA,  is  an  algorithm  that  uses  a  random 
sample  of  points  from  the  source  point  cloud  and  compares  them  to  similar  points  on  the  target 
point  cloud  and  the  output  is  a  homogeneous  transformation  matrix  that  would  roughly  align  the 
two  point  clouds.  The  goal  of  implementing  SAC-IA  as  part  of  the  overall  algorithm  is  to 
provide  an  estimated  transformation  that  will  be  used  as  an  initial  guess  for  the  Iterative  Closest 
Point,  or  ICP,  registration  method  (discussed  in  the  next  section)  to  find  the  global  minimum  of 
each  registration.  The  way  that  this  algorithm  works  is  that  there  is  a  user  defined  number  of 
random  points  chosen  from  the  source  point  cloud.  These  random  points  have  to  a  be  user 
specified  minimum  distance  from  each  other.  For  each  one  of  these  points,  the  neighboring  data 
points,  which  are  determined  by  a  user  defined  radius,  are  used  to  estimate  surface  normals  and 
features  needed  to  help  calculate  the  Fast  Point  Feature  Histograms  (FPFH)  (Rasu  et  al.  1992). 
Using  FPFHs,  SAC-IA  iterates  through  each  random  sample  from  the  source  point  cloud  and 
compares  that  point’s  features  to  points  in  the  target  point  cloud  until  there  are  similarities. 
When  this  happens,  for  each  random  sample  point  in  the  source  cloud,  a  list  of  points  in  the 
target  cloud  is  generated  based  off  similar  histograms.  Each  point  is  randomly  paired  up  with 
one  of  the  similar  points  from  the  target  cloud,  which  becomes  its  correspondence.  A  rigid 
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transformation  is  then  computed  between  the  sample  points  and  their  correspondences.  An  error 
metric  is  then  used  to  compute  the  quality  of  the  transformation.  These  three  steps,  seen  in 
Figure  13,  are  repeated,  and  the  transformation  attempt  that  had  the  smallest  error  is  stored  as  the 
output. 


1. 

2. 

3. 


Selects  user  defined  sample  points  (k)  from  source  cloud  while  making  sure  the  distances  between 
points  are  greater  than  the  user  given  parameter,  d  . 

For  each  sample  point,  find  a  list  of  points  in  the  target  cloud  whose  histograms  are  similar  to  the 
sample  points’  histogram.  From  these,  select  one  which  will  be  considered  that  sample  points 
correspondence. 

Compute  a  rigid  transformation  defined  by  the  sample  points  and  their  correspondences.  Calculate 
an  error  that  determines  the  quality  of  the  transformation. 


The  error  metric  for  the  third  step  is  determined  using  a  Huber  penalty 
r  i 


measure:  Lh(ei)  = 


-e 


IM  ^  te 

~te(2||ej||  —  te)  INI  >  te 


Figure  13:  SAC-IA  Algorithm  Psuedocode 
6.1. 1.2  Iterative  Closest  Point 

The  way  that  ICP  registration  method  works  is,  given  two  partially  overlapping  input 
point  clouds,  one  named  the  source  point  cloud  and  one  the  target  point  cloud,  it  uses  local 
optimization  to  compute  M-+1  in  order  to  minimize  the  “alignment  error”.  The  algorithm  also 
requires  an  initial  estimation  of  transformation  (which  in  this  algorithm  is  provided  by  SAC-IA) 
and  criteria  to  stop  the  iterations.  ICP  is  an  algorithm  that  iteratively  adjusts  the  transformation 
matrix  to  reduce  the  mean  square  error  between  the  target  and  source  point  clouds.  A  smaller 
average  squared  error  would  indicate  a  better  fit.  The  algorithm  has  four  steps,  which  can  be 
seen  in  Figure  14  (Besl  and  McKay  1992). 

The  ICP  algorithm  has  three  criteria  for  termination:  the  number  of  iterations  has  reached 
the  maximum  user  imposed  number  of  iterations,  the  difference  between  the  previous 
transformation  and  the  current  estimated  transformation  is  smaller  than  a  user  defined  value,  or 
the  mean  of  the  Euclidean  squared  errors  is  smaller  than  a  user  defined  threshold.  ICP  is 
extremely  efficient;  however,  the  problem  with  the  Iterative  Closest  Point  algorithm  is  that  if  the 
last  two  termination  criteria  are  never  met,  the  algorithm  often  settles  into  a  local  minimum 
instead  of  the  desired  global  minimum.  Essentially,  the  algorithm  iterates  back  and  forth  around 
a  local  minimum  and  its  surrounding  points  until  the  maximum  iterations  are  reached.  If  the 
initial  estimate  of  the  transform  matrix  is  poor,  the  ICP  algorithm  will  never  find  the  desired 
global  minimum.  Instead  of  inserting  a  random  or  generic  guess  for  each  registration  attempt,  it 
is  more  desirable  to  have  an  automatic  initial  approximate  alignment  transformation  step.  This 
transformation  would  have  to  be  accurate  enough  that  if  ICP  is  then  used,  ICP  can  find  the  global 
minimum.  This  is  the  reason  why  an  initial  estimate  is  required  as  an  input,  and  in  this  case  is 
provided  by  the  SAC-IA  algorithm. 
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1.  Get  input  point  clouds.  Declare  one  as  the  source  point  cloud  and  one  as  the  target 

2.  While  Termination  Criteria  ==  False 

a. 

For  each  point  in  the  target  cloud 

f 

i 

Find  the  nearest  point  in  the  source  cloud 

a. 

1 

Compute  the  transformation  from  the  source  point  cloud  onto  the  target  point  cloud 

b. 

Transform  source  point  cloud 

c. 

Compute  mean  squared  error 

Figure  14:  ICP  Algorithm  Psuedocode 

Figure  15  below  displays  all  the  user  defined  values  for  both  SAC-IA  and  ICP 
algorithms.  Appendix  A  provides  sample  values  for  each  user  defined  value  on  various  clouds, 
along  with  the  resulting  transformation  matrix.  Figure  16  below  shows  the  same  two  clouds 
from  Figure  12;  however,  in  this  image,  the  source  cloud  has  been  multiplied  by  the 
transformation  matrix  that  has  been  produced  through  the  combined  efforts  of  the  SAC-IA  and 
ICP  registration  methods  resulting  in  two  aligned  point  clouds. 
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User  Determined  SAC-IA  Parameters 

•  Number  of  Samples:  Sets  the  number  of  random  samples  (k)  that  will  be  selected 
from  the  source  point  cloud 

•  Normal  Radius:  Sets  the  radius  for  the  normal  search  around  each  random 
sample. 

•  Feature  Radius:  Sets  the  radius  for  the  feature  search  around  each  random 
sample. 

•  Minimum  Sample  Distance:  Sets  the  minimum  distance  threshold  between  the 
randomly  selected  points. 

•  Maximum  Correspondence  Distance:  Allowable  error  for  code  to  terminate 
before  maximum  number  of  iterations  is  reached. 

•  Maximum  Iterations:  Set  the  maximum  number  of  iterations  for  the  algorithm  to 
go  through  before  termination. 

User  Determined  ICP  Parameters 

•  Maximum  Correspondence  Distance:  Sets  the  maximum  distance  threshold 
between  two  correspondent  points  in  the  source  and  target  clouds.  If  the 
distance  between  two  corresponding  points  is  larger  than  this  threshold,  the 
points  will  be  ignored  in  the  alignment  process. 

•  Transformation  Epsilon:  The  transformation  epsilon  is  the  allowable  difference 
between  two  consecutive  transformations.  The  algorithm  terminates  when  the 
transformation  epsilon  falls  below  this  user  determined  value. 

•  Maximum  Iterations:  Set  the  maximum  number  of  iterations  for  the  algorithm  to 
go  through  before  termination. 

NOTE:  Maximum  correspondence  distance  for  SAC-IA  is  not  the  same  as  ICP 
Figure  15:  User  defined  values  for  the  SAC-IA  and  ICP  Algorithms 
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Figure  16:  Two  aligned  point  clouds  as  the  result  of  SAC-IA  and  ICP  registration  algorithms 
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6.1.2  Concatenation 

At  this  point,  there  are  two  scans  that  are  aligned,  but  they  are  still  two  scans.  The  rest  of 
the  algorithm  uses  one  dense  point  cloud  that  contains  enough  data  to  identify  a  dock  as  the 
primary  input.  Concatenation  is  the  process  of  combining  these  two  scans  together.  To  facilitate 
the  concatenation  of  scans,  the  process  used  was  to  select  multiple  point  clouds  that  were 
collected  in  a  line  (such  as  down  a  dock  or  along  the  shoreline)  and  align  them  to  one  point 
cloud.  This  was  done  with  six  different  point  clouds  spaced  three  meters  along  the  target  dock  to 
form  a  point  cloud  in  which  a  dock  was  easily  identifiable.  The  resulting  point  cloud  is  robust 
enough  to  use  to  identify  a  dock.  Figure  17  is  a  screenshot  of  six  point  clouds  that  were  collected 
along  the  dock,  registered,  and  concatenated  together. 
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Figure  17:  Dense  point  cloud  created  from  six  different  scans 
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6.2  Shore  Removal 

Docks  have  distinct  shapes  in  3-D  LiDAR  scans.  The  LiDAR  sensors  are  not  able  to 
obtain  information  from  the  water  because  the  water  refracts  the  lasers,  so  the  data  does  not 
return  to  the  sensor.  The  docks  create  thin  peninsulas,  as  seen  in  the  previous  figure,  which 
protrude  from  the  shoreline.  The  objective  of  shore  removal  is  to  isolate  these  distinct  shapes;  if 
this  can  happen,  it  greatly  reduces  the  amount  of  data  that  has  to  be  examined  by  the  rest  of  the 
algorithm.  In  order  to  remove  the  shore,  image  processing  techniques  were  used. 

Mathematical  morphology  is  an  image  processing  tool  used  for  extracting  image 
components  that  are  useful  in  the  representation  and  description  of  region  shapes  (Gonzalez  et  al. 
2009).  Many  different  logical  operations  fall  under  the  category  of  morphology,  two  of  which 
are  used  in  this  algorithm  are  dilation  and  erosion.  These  two  processes  are  used  on  binary 
images.  A  binary  image  is  a  logical  image  in  which  the  foreground  is  true  (pixel  value  of  1)  and 
the  background  is  false  (pixel  value  of  0).  Dilation  is  an  operation  that  “grows”  or  thickens” 
objects  in  a  binary  image  while  erosion  is  an  operation  that  “shrinks”  or  “thins”  objects  in  a 
binary  image.  The  amount  an  object  is  “grown”  or  “thinned”  is  dependent  on  a  user  defined 
structuring  element.  A  structuring  element  is  a  shape  defined  by  a  small  matrix  used  to  interact 
with  the  binary  image  that  has  a  defined  origin.  For  dilation,  the  structuring  element’s  origin  is 
translated  throughout  the  plane  of  the  image,  and  wherever  the  origin  aligns  with  a  1 -valued 
pixel,  all  the  other  pixels  in  the  image  that  are  under  the  structuring  element  also  become  a  1- 
valued.  Erosion  is  almost  the  opposite,  it  iterates  through  all  the  pixels  in  the  image,  and  when  its 
origin  lands  on  a  1 -valued  pixel  the  pixels  inside  the  structuring  element  become  zero  valued. 

Figure  18  shows  eight  images  of  each  stage  of  this  process,  and  the  next  section 
methodically  goes  over  the  steps  in  greater  detail.  In  this  algorithm,  morphology  plays  a  role  in 
trying  to  remove  the  shore.  The  basic  concept  of  what  is  done  is  that  a  binary  image  is  created 
using  the  X  and  Y  coordinates  of  every  point  in  the  cloud.  Using  only  the  X  and  Y  coordinates 
of  each  point  in  the  point  cloud  allows  the  algorithm  to  create  a  binary  image  of  the  overhead 
view  of  the  area.  This  overhead  view  reveals  the  docks  as  penisulas  and  because  of  this  unique 
feature,  the  algorithm  tries  to  eliminate  the  shore  through  morphological  processes.  After  the 
overhead  image  is  created,  the  image  is  eroded  using  a  structuring  element  that  is  twice  as  big  as 
the  target  docks  width  plus  one.  The  reason  for  this  is  to  ensure  that  the  entirety  of  the  dock  gets 
eroded.  At  this  point,  the  eroded  image  is  dilated  in  order  to  return  the  shore  to  its  original  size; 
however,  this  dilated  image  does  not  contain  the  docks,  yet  still  retains  the  general  shape  of  the 
shoreline.  If  this  new  image  is  subtracted  from  the  original  image,  then  the  output  image  is  the 
original  image  with  the  shore  removed.  This  final  image  is  then  used  to  create  a  new  point  cloud 
based  that  is  used  in  the  next  step  of  the  algorithm. 
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Figure  18:  Eight  images  that  correspond  with  the  eight  steps  of  shore  removal 
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1.  Image  1  in  Figure  18  shows  the  original  dense  point  cloud  from  an  overhead  view. 

This  is  to  gain  the  perspective  of  the  X  and  Y  values  of  the  point  cloud  that  will  be 
used  to  convert  the  point  cloud  into  a  binary  image. 

2.  Image  2  exhibits  the  binary  image  obtained  from  the  point  cloud  in  Image  1  using  its  X 
and  Y  values.  In  this  image  each  pixel  represents  a  lmx  lm  area  of  the  point  cloud, 
and  if  a  point  was  determined  to  be  in  that  one  by  one  meter  area,  then  the  pixel 
becomes  a  1,  or  white.  Figure  19  shows  the  pseudocode  for  the  conversion  between 
the  point  cloud  and  the  binary  image,  the  C++  code  can  be  seen  in  Appendix  B. 


Point  Cloud  to  Binary  Image  Conversion  Psuedocode 

a)  Determine  minimum  and  maximum  X  and  Y  values  of  the  point  cloud 

b)  Round  the  maximum  values  up  to  the  next  integer  and  round  the  minimum  values 
down  to  the  next  integer 

c)  Obtain  the  absolute  size  in  the  X  and  Y  direction 

d)  Set  pixel  size  to  correspond  to  a  real  world  distance 

e)  Create  a  binary  image  using  the  each  values  absolute  size,  but  add  a  value  of  two 
in  each  direction  so  that  no  part  of  the  point  cloud  lies  on  the  edge  of  the  binary 
image 

f)  For  every  pixel  in  the  Y  direction 

{ 

For  every  Pixel  in  the  X  direction 

t 

For  every  point  in  the  point  cloud 


If  the  X  and  Y  coordinates  of  the  point  cloud 
correspond  the  current  pixel 

i 

Make  the  current  pixel  true 


Figure  19:  Point  cloud  to  binary  image  conversion  psuedocode 

3.  Image  3  displays  the  newly  created  binary  image,  but  with  the  smaller  individual  pixels 
removed.  Small  groups  of  1 -valued  pixels  in  a  binary  image  can  often  cause  undesired 
results  when  using  morphological  functions.  This  is  because  these  small  clusters  of 
pixels  often  have  little  to  no  utility  when  trying  to  identify  a  dock;  however,  when 
morphological  functions,  such  as  dilation,  are  applied,  they  can  connect  to  larger 
objects  in  the  image.  In  the  case  of  this  algorithm,  these  individual  pixels  represent 
data  points  from  the  LiDAR  scan  that  are  not  important  to  identifying  the  dock.  It  is 
assumed  that  at  this  point  in  the  code,  the  dock  and  all  of  the  information  that  needs  to 
be  extracted  from  it  will  be  represented  in  the  image  by  more  than  one  pixel.  This  is 
why  small,  non-connected  objects  are  removed  at  this  point.  In  order  to  do  this,  the 
area  of  each  object  in  the  image  is  determined  by  using  OpenCV  contours.  If  the  area 
of  any  given  shape  in  the  image  is  smaller  than  a  user  defined  value,  then  those  pixels 
are  removed.  In  this  algorithm,  the  defined  value  was  twenty  pixels. 

4.  The  next  step  in  the  process  is  to  dilate  the  image  in  order  to  fill  in  the  gaps  in  the 
objects  so  that  the  whole  foreground  is  one  object.  If  there  are  gaps  in  the  middle  of 
the  objects,  it  was  determined  that  the  gaps  were  part  of  the  shore  anyway,  so  cleaning 
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the  image  up  and  filling  those  gaps  creates  a  cleaner  output  at  the  end  of  the  process. 

In  MATLAB  there  is  a  function  called  “imfill”  which  does  just  that;  however,  when 
working  with  C++  and  OpenCV,  there  is  no  such  function.  Instead  of  finding  pre¬ 
written  function  or  writing  a  new  one,  it  was  determined  that  a  simple  dilation  could 
complete  the  objective  while  not  interfering  with  the  overall  process.  The  structuring 
element  that  was  used  represents  a  2.25  by  2.25meter  square.  This  means  dilation 
process  is  expanding  the  shore  by  1.125  meters  in  every  direction  and  filling  in  the 
middle  holes. 

5.  The  process  from  Image  4  to  Image  5  is  two  iterations  of  erosion  using  the  same  size 
element.  The  dock  is  no  more  than  two  meters  across,  so  by  using  a  structuring 
element  that  reduces  the  image  in  size  by  a  relative  1.125  meters,  the  dock  will  be 
eliminated  on  the  second  iteration.  The  first  iteration  takes  the  previously  dilated 
image  back  to  its  original  size;  however,  the  second  erosion  is  what  eliminates  the 
docks  that  are  thinner  than  the  structuring  element. 

6.  Image  6  is  the  eroded  shore,  with  the  two  skinny  docks  removed,  dilated  one  iteration 
by  the  previously  sized  structuring  element.  This  dilation  swelled  the  eroded  shore 
back  to  the  approximate  size  of  the  shore  in  the  original  image. 

7.  Image  7  is  Image  6  subtracted  from  Image  2.  This  step  essentially  removes  the  shore 
from  the  image  leaving  the  thin  docks  as  the  two  largest  objects.  There  is  a  lot  of  other 
smaller  objects  that  are  part  of  Image  7,  but  that  is  a  reasonable  expectation.  It  is 
expected  that  no  morphological  process  will  be  robust  and  perfect  when  applied  to 
varius  scenes. 

8.  Image  8  is  the  overhead  view  of  the  point  cloud  that  is  created  from  the  binary  pixels  in 
Image  7.  For  this  step,  it  iterates  through  every  point  in  the  original  point  cloud  and 
includes  all  points  whose  X  and  y  values  correspond  to  the  true  pixels  in  Image  7.  This 
cloud  now  has  fewer  data  points  and  will  be  used  as  the  input  for  the  next  step  of  the 
algorithm,  which  is  finding  the  plane  of  the  dock. 


6.3  Finding  the  Plane  of  the  Dock 

In  the  assumptions,  it  was  stated  that  each  dock  would  have  a  planar  surface  surrounded  by 
repeated  pilings.  Identifying  the  plane  and  removing  it  allows  the  pilings  to  be  identified  as 
explained  in  the  next  section. 

In  order  to  find  the  planar  surface  in  the  point  cloud,  Random  Sample  Consensus 
(RANSAC),  is  the  algorithm  used  to  estimate  the  best  fit  plane  to  the  data  set.  RANSAC  is  an 
iterative  method  that  estimates  parameters  of  a  mathematical  model  (i.e.  the  equation  of  a  plane 
in  this  case)  given  a  set  of  noisy  input  data  that  contains  both  inlier  and  outlier  data  points 
(Fishier  and  Bolles  1981).  Point  Cloud  Library  has  a  class  named  “Segmentation”.  The  class’s 
job  is  to  be  able  to  segment  desired  data  from  a  larger  input  data  cloud.  Given  an  input  data  set, 
the  goal  of  the  segmentation  class  is  to  find  inlier  data  points  that  meet  certain  criteria.  The 
criteria  are  based  on  the  Euclidean  distance  of  the  points  from  the  best  fit  plane  model.  The  way 
the  PCL  implementation  of  RANSAC  works  is  that  the  user  has  to  input  a  model  type,  a  method 
type,  a  threshold,  and  maximum  iterations.  The  model  type  is  the  object  that  one  is  trying  to 
segment  from  the  point  cloud,  as  stated  earlier,  this  is  the  model  of  the  plane.  Method  type  is 
which  robust  estimator  to  use;  in  this  case,  RANSAC  is  being  used.  The  threshold  is  the  distance 
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allowance  to  find  inliers.  For  this  algorithm,  the  goal  is  to  find  the  best  fit  plane  using  a  0.25 
meter  threshold.  This  was  determined  as  the  best  threshold  because  it  captures  most  of  the 
planar  surface  of  the  dock,  but  still  left  the  pilings  as  outliers.  Figure  20  below  shows  the  input 
cloud  after  the  shore  removal  process,  while  Figure  21  shows  the  same  cloud  below,  but  with  the 
planar  inliers  highlighted  as  red. 
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Figure  20:  Point  cloud  with  eliminated  shore 
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Figure  21:  Shows  the  point  cloud  from  Figure  20  with  the  dock  identified  in  red 
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6.4  Identifying  the  Pilings 

6.4.1  Extracting  the  Plane  and  Cluster  Recognition 

The  first  step  to  identify  the  pilings  is  to  remove  the  recently  found  plane.  The  reason  for 
doing  this  is  that  once  the  plane  is  removed,  the  pilings  will  be  the  only  data  points  remaining  of 
the  dock.  The  segmentation  class  of  Point  Cloud  Library  allows  for  the  extraction  of  points  from 
a  point  cloud,  so  to  extract  the  plane,  all  that  needed  to  be  done  was  extract  the  point  cloud  that 
contained  the  planar  inliers.  The  result  can  be  seen  in  Figure  23. 


At  this  point,  the  next  step  is  to  cluster  the  data  points  of  each  piling  together.  Point 
Cloud  Library  offers  an  Euclidean  Cluster  Extraction  as  part  of  their  segmentation  class.  What 
Euclidean  Cluster  Extraction  does  is  divides  an  unorganized  point  cloud  into  smaller  organized 
parts.  These  organized  clouds  can  then  be  individually  evaluated  to  determine  if  they  meet  the 
parameters  of  specific  objects,  such  as  pilings  Feng  and  Song  2008).  These  smaller  organized 
parts  are  determined  by  user  inputs.  The  goal  of  implementing  Euclidean  Cluster  recognition  is 
to  seperate  each  piling  as  its  own  cluster  and  then  evaluate  its  shape  to  determine  if  it  is  a  piling 
The  Euclidean  Cluster  Recognition  function  that  is  used  in  this  algorithm  applies  a  neighbor 
search  technique.  This  means  that  for  each  point  in  the  input  cloud  (the  point  cloud  with  the 
plane  removed,  see  Figure  23),  the  points  are  evaluated  to  find  its  neighbors  within  a  distance 


threshold.  The  distance  threshold  represents  the 
radius  of  a  sphere  that  encloses  the  given  data 
point.  If  a  neighboring  data  point  lies  within 
this  spherical  area,  the  two  points  are  grouped 
together  into  a  cluster.  This  process  continues 
from  each  point  in  the  inputted  cloud  until 
every  point  is  part  of  a  cluster.  For  this 
algorithm,  the  distance  threshold  was  30 
centimeters.  This  distance  was  chosen  because 
as  seen  in  Figure  22,  when  the  plane  is 
removed,  it  extracts  the  data  from  the  piling 
with  it.  Thirty  centimeters  allows  the  data 
points  to  cluster  together  over  the  chasm  caused 
by  the  extraction  of  the  plane,  but  does  not 
allow  the  data  points  of  one  piling  to  cluster 
with  another  piling  since  the  distance  between 
pilings  is  three  meters.  Next  the  cluster  size  is 
evaluated.  If  the  cluster  has  fewer  than  five 
data  points,  or  more  than  two  hundred,  the 
cluster  is  dropped.  No  identifiable  piling  will 
have  fewer  than  five  data  points,  and  none  of 
the  pilings  have  more  than  two  hundred  points. 
This  eliminates  some  excess  data  before  it  is 
further  processed. 

The  next  step  is  to  obtain  the  whole 
piling.  As  previously  mentioned,  when  the 


Figure  22:  Piling  on  left  has  planar  data  points 
removed  (area  between  the  yellow  lines)  while 
piling  on  right  has  data  points  from  plane 
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planar  inliers  are  extracted,  it  leaves  a  gap  in  the  piling  data,  which  means  that  the  current  cluster 
does  not  represent  a  whole  piling.  This  step  is  to  go  back  and  add  the  data  from  the  plane  to  the 
clusters.  In  order  to  do  this,  the  code  iterates  through  each  cluster  and  determines  a  bounding 
box  in  the  X  and  Y  plane.  Any  point  that  is  in  the  planar  point  cloud  that  is  within  the  limits  of  a 
cluster’s  X  and  Y  bounding  box  is  then  added  to  that  respective  cluster.  Figure  22  shows  the 
before  and  after  effects  of  this  part  of  the  code  on  a  piling  close  up.  The  second  image  clearly 
has  more  points  and  is  more  representative  of  a  piling  than  the  first  image. 
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Figure  23:  Point  cloud  from  Figure  21  with  plane  removed 
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6.4.2  Using  a  Gaussian  Probability  Density  Function  and  Bayes’  Theorem  to  Identify  Pilings 

Once  each  cluster  has  had  the  planar  data  points  re-inserted,  the  next  step  to  identify  the 
pilings  is  to  evaluate  each  cluster  to  determine  the  probability  it  is  a  piling.  Past  research  has 
shown  that  using  Bayesian  Pattern  Recognition  and  other  probability  functions  have  proven 
successful  when  trying  to  identify  different  shapes  in  unorganized  point  clouds  (Schnabel  et  al. 
2006).  Once  each  cluster  has  a  probability  of  being  a  piling  associated  with  it,  only  those  that 
meet  a  user  defined  threshold  are  considered  pilings. 

The  first  step  to  identifying  a  piling  is  determining  what  features  are  useful  to  differentiate  a 
piling  from  other  clusters.  Seven  features  were  determined  to  be  useful  in  this  regard: 

1.  centroid  height; 

2.  vertical  separation  of  data  points  in  the  piling  (height  of  pilings); 

3.  large  diameter  in  the  X-Y  plane; 

4.  short  diameter  in  the  X-Y  plane; 

5.  ratio  of  the  two  diameters; 

6.  ratio  of  the  height  of  the  piling  over  the  ratio  of  the  diameters;  and 

7.  ratio  of  the  height  over  the  average  of  the  diameters. 

In  order  to  compute  these  features,  the  centroids  and  three-dimensional  covariance  matrices 
were  determined  for  each  cluster.  A  three  dimensional  covariance  matrix  is  3x3  matrix.  The 
diagonal  of  the  covariance  matrix  represents  the  variance  in  each  dimension,  where  the  [1,1] 
element  would  represent  the  variance  in  the  X  direction  while  [3,3]  element  represents  the 
variance  in  the  Z  direction.  Below  are  the  reasons  why  each  feature  was  chosen  and  the  steps  to 
obtain  that  feature  for  each  cluster. 

1.  Centroid  Height  -  This  feature  was  chosen  because  the  pilings  are  always  located  near 
the  water,  so  they  will  always  show  up  around  the  same  height.  By  evaluating  centroid 
height  as  a  feature,  it  takes  vertical  position  into  account  and  can  help  eliminate  any 
clusters  that  may  resemble  a  piling  but  not  match  the  vertical  position  of  a  piling,  such  as 
a  tree  trunk.  This  value  is  obtained  by  simply  taking  the  Z  coordinate  from  the 
previously  calculated  centroid. 

2.  Vertical  Range  of  Data  Points  in  the  Piling  (Height  of  Pilings)  -  This  feature  is  the 
approximate  height  of  the  piling.  Evaluating  this  feature  for  each  cluster  identifies  the 
vertical  separation  of  the  data  points  which  is  advantageous  for  eliminating  clusters  that 
are  too  tall  or  too  short.  To  obtain  this  value,  the  information  is  extracted  from  the 
covariance  matrix  that  is  determined  for  each  cluster.  By  obtaining  the  third  row  and 
third  element  of  the  covariance  matrix  produced  by  each  cluster,  the  variance  in  the  Z 
direction  can  be  obtained.  The  square  root  of  the  variance  is  then  taken  to  obtain  the 
standard  deviation.  The  standard  deviation  is  then  multiplied  by  six  to  determine  the 
approximate  height.  Assuming  a  Gaussian  distribution,  three  standard  deviations  is  a 
plus  or  minus  98.5%  confidence  interval.  This  step  utilizes  the  eigenvalues  in  the  X-Y 
plane  in  order  to  find  the  diameter  of  the  pilings.  A  perfect  piling  should  have  the  same 
diameter  throughout  the  piling  because  a  piling  is  essentially  a  cylinder.  However,  what 
happens  with  point  cloud  data  is  that  a  LiDAR  scan  can  normally  only  see  half  the  piling 
on  one  scan.  That  is  why  features  three  and  four  are  long  and  short  diameters.  In  order 
to  obtain  the  diameters  of  each  cluster,  the  top  left  two  by  two  matrix  of  the  calculated 
three  dimensional  covariance  matrix  is  extracted.  This  two  by  two  matrix  shows  only  the 
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variation  between  the  X  and  Y  dimensions.  Using  this  covariance  matrix,  two 
eigenvalues  are  calculated.  One  represents  the  variance  of  the  longest  distance  across  the 
X-Y  plane  of  each  cluster  while  the  other  one  represents  the  shortest  value’s  variance. 
Using  the  same  statistical  technique  applied  in  step  two  where  the  square  root  of  the 
variance  value  is  obtained  then  multiplied  by  six,  the  approximate  values  for  each 
cluster’s  long  and  short  diameter  are  determined. 

3.  The  fourth  feature  calculated  for  each  cluster  is  the  shortest  diameter.  This  is  calculated 
the  same  way  as  feature  3. 

4.  The  fifth  feature  is  a  ratio  of  the  two  diameters.  The  ratio  is  a  good  indicator  of  how 
symmetrical  the  cluster  is.  This  ratio  was  calculated  by  simply  diving  the  large  diameter 
by  the  short  diameter. 

5.  Feature  six  is  the  height  of  the  data  points  from  feature  two,  divided  by  the  ratio  of  the 
diameters.  The  reason  this  is  a  feature,  along  with  feature  seven,  is  that  in  general, 
Bayesian  classifiers  will  have  greater  discriminatory  power  when  a  larger  number  of 
features  are  used,  especially  when  the  features  are  not  linearly  dependent. 

6.  The  seventh  feature  is  the  ratio  of  the  height  of  the  data  points  divided  by  the  average 
between  the  two  determined  diameters. 


The  equation  for  the  Gaussian  Probability  Density  Function  (PDF)  is 
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where  Cj  and  mj  are  the  covariance  matrix  and  mean  vector  of  the  pattern  population  of  class  Wj 
and  ICjl  is  the  determinant  of  Cj.  In  general,  a  Gaussian  PDF  computes  a  PDF  value  evaluating 
one  data  set  to  a  known  or  “trained”  dataset.  The  output  PDF  value  is  high  if  the  two  datasets  are 
similar,  and  likewise  low  if  the  two  datasets  are  different.  In  this  equation,  class  j  represents  the 
“trained”  data.  In  order  to  implement  Bayes’  Theorem,  each  cluster  needs  to  be  evaluated 
through  two  Gaussian  PDFs,  one  evaluates  the  probability  that  the  cluster  is  a  piling,  and  one 
calculates  the  probability  that  the  cluster  is  not  a  piling.  For  each  of  these  PDFs,  supervised 
training  data  about  each  class  must  be  collected.  This  means  that  data  has  to  be  gathered  from 
clusters  that  are  known  pilings  in  a  point  cloud,  and  likewise  for  clusters  that  are  known  non¬ 
pilings.  The  Gaussian  PDF  equation  is  also  useful  for  determining  the  global  PDF  value,  p(x), 
which  is  a  global  PDF  value  for  all  classes  combined.  This  value  is  later  used  in  Bayes’ 

Theorem. 


The  training  data  was  collected  by  selecting  certain  clusters  from  the  data  that  were 
known  to  be  pilings  or  not  pilings.  For  each  known  cluster  of  either  class,  the  features  listed 
above  were  calculated.  Then  a  covariance  matrix  and  mean  vector  were  calculated  from  the  data 
for  each  class.  The  covariance  matrix  and  mean  vector  for  each  class  were  then  used  in  their 
class’s  respective  PDF  for  calculating  the  probability  of  that  class.  Using  the  known  information 
for  each  population  class,  the  PDF  value  of  a  cluster  for  each  class  is  determined  by  evaluating 
that  cluster’s  features  in  each  of  the  PDFs. 
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Once  the  probability  for  each  class  is  determined  for  one  cluster,  the  algorithm  utilizes 
Bayes’  Theorem  to  get  the  overall  probability  that  the  cluster  is  a  piling.  Bayes’  Theorem, 
produces  a  posterior  probability  that  the  observation  X  is  a  member  of  class  wi,  seen  below: 

'  P(Wl\x)  = 

p  (x) 

In  this  algorithm,  Bayes’  Theorem  will  only  deal  with  two  population  classes,  “Pilings”,  wi,  and 
“Not  Pilings”,  w2.  For  Bayes’  Theorem  to  work,  each  of  these  two  classes  requires  a  prior 
probability,  or  the  likelihood  of  encountering  a  particular  class  within  the  overall  population. 

This  captures  the  frequency  with  which  pillings  appear  in  a  typical  point  cloud  data  set,  relative 
to  other  clusters  such  as  cars,  trees,  boulders,  etc.  In  this  algorithm,  these  values  were 
determined  by  examining  an  ideal  point  cloud  data  set  and  determining  that  there  were  twenty- 
two  pilings  that  should  be  identified  with  the  given  information  out  of  125  clusters.  From  this 
information,  the  prior  probability  was  set  to  be: 

p(Wl)  =  (22/i25), 

P(w2)  =  (103/ ^25)* 

Once  each  cluster  is  evaluated  and  a  probability  of  that  cluster  being  a  piling  is  produced, 
the  algorithm  selects  all  the  clusters  whose  probability  is  over  a  certain  threshold.  For  the 
clusters  that  are  over  the  threshold,  they  are  combined  into  a  single  point  cloud  while  the  other 
clusters  are  discarded.  The  threshold  that  is  used  in  this  algorithm  is  a  50%  value,  meaning  that 
if  Bayes’  Theorem  believes  that  there  is  greater  than  a  fifty  percent  chance  that  the  cluster  is  a 
piling,  the  algorithm  then  labels  the  cluster  as  an  identified  piling.  Figure  24  shows  the  identified 
pilings  from  the  current  point  cloud  highlighted  in  red. 
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Figure  24:  Identified  pilings  are  highlighted  in  red 
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6.5  Compare  Identified  Pilings  with  Objects  Removed  from  the  Shore 

The  next  step  of  the  algorithm  is  to  check  the  spatial  arrangement  of  the  pilings.  One  of  the 
assumptions  is  that  each  dock  identified  will  have  pilings  regularly  spaced  along  each  side  of  its 
planar  surface.  Morphology  is  used  again  to  check  the  proximity  of  pilings.  The  algorithm 
assumes  that  the  if  the  pilings  are  close  enough  to  each  other,  then  a  dilation  function  will  create 
a  single  object  in  a  binary  image.  The  area  that  is  then  identified  with  the  pilings  is  multiplied 
with  the  binary  image  of  the  shore  eliminated.  This  multiplication  ensures  that  the  only  points 
that  are  converted  into  the  final  point  cloud  are  points  that  are  not  part  of  the  shore  and  are  in 
between  the  closely  spaced  pilings.  This  process  only  has  six  steps,  which  can  be  seen  in  Figure 
25.  The  steps  are  as  follows: 

1.  Convert  the  point  cloud  of  the  pilings  (Image  1)  to  a  binary  image  (Image  2).  This  is 
using  the  same  process  that  was  used  for  the  shore  removal  step. 

2.  The  next  step  is  to  dilate  the  objects  in  the  image.  The  goal  is  to  dilate  each  piling 
enough  so  that  it  will  connect  with  its  neighboring  pilings  when  they  are  dilated.  Since 
the  distance  between  pilings  is  three  meters,  the  structuring  element  size  represented  1.75 
meters.  This  allows  closely  spaced  pilings  to  become  one  object. 

3.  The  next  step  is  to  erode  the  objects.  Instead  of  eroding  the  objects  using  the  same  size 
structuring  element  as  in  the  dilation  step,  the  objects  are  eroded  using  a  smaller  sized 
structuring  element.  The  purpose  of  this  is  to  allow  a  buffer  zone  around  the  pilings  for 
the  next  step.  The  structuring  element  used  erodes  pixels  that  represent  0.75  meters  of 
the  dilated  object.  This  leaves  a  one  meter  buffer  zone  around  the  original  pilings.  Image 
3  in  Figure  25  shows  the  result  of  steps  two  and  three. 

4.  The  next  step  is  to  multiply  this  image  of  the  combined  pilings  by  the  previously  obtained 
image  of  the  removed  shore,  Image  4.  This  allows  only  the  pixels  through  which  are  both 
true,  Image  5.  This  is  the  reason  it  is  acceptable  to  have  a  buffer  zone  around  the  pilings 
from  step  three. 

5.  For  all  the  pixels  that  remain  true  from  step  four,  create  a  point  cloud  that  represents  all 
the  points  in  the  final  binary  image’s  corresponding  region. 
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Figure  25:  Morphological  steps  to  check  roximity  of  pilings. 
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6.6  Evaluate  Identified  Point  Clouds 

In  some  tests  of  different  point  clouds,  it  was  noticed  that  sometimes  the  piling 
identification  step  would  produce  false  positives,  or  it  would  find  a  real  piling  on  another  dock, 
but  could  not  identify  its  neighboring  pilings.  This  created  issues  with  the  final  point  cloud,  see 
Figure  26.  If  there  was  a  lone  piling  and  that  is  not  part  of  the  shore,  the  algorithm  would  allow 
its  surrounding  points  to  be  identified  as  a  dock.  The  problem  with  this  is  that,  while  the  piling 
may  be  part  of  the  dock,  it  is  not  a  suitable  dock  location,  which  is  what  the  algorithm  is  truly 
trying  to  identify.  This  last  step  is  to  eliminate  these  issues. 


Figure  26:  Part  of  the  seawall  identified  as  a  false  positive 


The  assumption  called  for  the  docks  to  be  long,  narrow,  and  skinny.  At  this  point  in  the 
algorithm,  a  single  piling  does  not  represent  a  dock  in  the  sense  that  the  point  cloud  does  not 
meet  the  previously  mentioned  descriptors.  In  order  to  check  if  the  dock  is  long,  narrow,  and 
skinny,  the  output  point  cloud  is  clustered  using  parameters  where  each  cluster  represents  an 
object,  whether  it  be  the  dock,  or  a  single  piling.  Eigenvalues  are  then  calculated  for  each 
cluster.  The  ratio  of  the  long  eigenvalue  over  the  short  eigenvalue  is  determined.  This  ratio 
represents  the  length  to  width  ratio  of  the  point  cloud.  It  was  determined  that  the  ratio  should  be 
at  least  two,  meaning  that  the  dock  location  should  be  twice  as  long  as  it  is  wide.  However, 
sometimes  objects  are  small  and  really  skinny,  such  as  the  sea  wall  in  Figure  26,  which  would 
produce  a  high  ratio.  For  this  reason,  the  small  eigenvalue  had  to  be  larger  than  a  certain  value. 
This  ensures  that  a  cluster’s  width  is  actually  wide  enough  to  be  a  dock.  Once  each  cluster  has 
gone  through  this  process,  the  ones  that  meet  the  thresholds  are  pushed  through  and  identified  as 
docks;  likewise,  the  clusters  that  do  not  meet  the  thresholds  are  discarded.  Figure  27  below 
exhibits  the  final  output  with  the  identified  dock  in  red. 
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Figure  27 :  Shows  the  original  point  cloud  with  the  dock  identified  in  red 
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7  Results 

From  the  previous  figure,  it  is  seen  that  identifying  a  suitable  dock  is  possible;  however,  this 
scan  is  not  a  realistic  scan.  The  data  for  the  previous  scan  was  collected  while  standing  on  top  of 
the  target  dock;  this  is  the  reason  why  the  laser  scans  are  perpendicular  with  the  length  of  the 
dock.  Collecting  data  on  top  of  the  dock  allowed  for  a  series  of  very  clean  scans  which  resulted 
in  a  clean  dense  point  cloud  once  all  were  transformed  and  concatenated.  While  this  scan  may 
not  represent  the  ideal  test  scan  for  the  applicability  of  an  autonomous  surface  vessel,  limitations 
in  the  algorithm  can  still  be  found.  In  Figure  27,  it  can  be  seen  that  the  portion  of  dock  close  to 
the  shore  was  not  identified.  The  reason  for  this  originates  in  the  shore  removal  step  and  the 
morphological  steps.  When  the  binary  image  of  the  point  cloud  is  originally  dilated  in  order  to 
fill  in  the  gaps  in  the  middle  of  objects,  the  shore  connects  with  the  dock  and  essentially  extends 
the  shore  the  length  of  the  structuring  element.  In  any  scan,  if  the  dock  is  not  perfectly 
perpendicular  with  the  shore  or  in  a  concave  shape  close  enough  for  the  structuring  element  to 
combine  the  two,  part  of  the  base  of  the  dock  will  not  be  identified.  However,  while  this  is 
important,  it  only  means  that  a  targeted  dock  must  protrude  away  from  the  shore. 

As  just  stated,  the  first  scan  was  not  a  realistic  scan  to  prove  this  algorithm’s  applicability  to 
autonomous  surface  vessels.  The  next  step  was  to  test  the  algorithm  on  a  scan  collected  from 
another  angle.  Figure  28  shows  an  image  of  where  the  data  was  collected  for  this  scan.  Notice 
how  the  line  is  perpendicular  with  the  dock  which  provides  a  new  perspective  for  the  algorithm. 
Figure  29,  previously  seen  as  Figure  26,  shows  the  results  of  the  algorithm  used  on  this  scan. 


Figure  28:  Data  collected  for  Figure  29  along  the  yellow  arrow 
Imagery  ©2013  U.S.  Geological  Survey.  Map  data  ©2013  Google 
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Figure  29:  Shows  identification  of  dock  wheredata  points  are  dense  enough  and  a  false  positive  along  the  sea  wall 
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There  are  many  points  of  interest  in  Figure  29.  First  note  that  the  algorithm  did  identify 
the  dock  in  some  places.  However,  it  is  important  to  realize  why  the  dock  was  not  identified  in 
its  entirety.  All  the  pilings  were  not  detected.  This  problem  arises  from  there  not  being  enough 
points  hitting  all  the  pilings.  Since  the  data  was  collected  perpendicular  with  the  dock  along  one 
line,  the  scan  provides  very  dense  data  in  the  location  in  front  of  the  sensor;  however,  the  sensor 
failed  to  collect  enough  information  about  the  end  of  the  dock  to  identify  anything  as  a  piling. 
One  of  the  most  notable  features  in  Figure  29  is  that  part  of  the  seawall  was  identified  as  a  dock. 
The  reason  that  it  is  identified  is  that  through  the  morphological  and  clustering  process,  the 
algorithm  believed  that  there  was  a  piling.  Also,  since  there  is  a  shadow  area  behind  the  cluster, 
the  shore  removal  step  did  not  remove  the  area  as  part  of  the  shore.  However,  the  image 
presented  in  Figure  29  was  captured  before  the  thresholds  that  evaluate  each  identified  cluster 
were  inserted  into  the  algorithm.  By  inserting  the  thresholds,  this  false  positive  is  eliminated  as  a 
suitable  docking  location.  The  same  seen  can  be  seen  below  in  Figure  30  with  the  false  positive 
eliminated. 


Figure  30:  Algortihm  output  with  false  positive  removed 
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The  next  scan  that  was  tested  was  a  scan  whose  data  was  collected  by  moving  the  cart 
along  the  dock  that  is  parallel  with  the  target  dock,  see  Figure  31.  The  previous  scan  proved  that 
a  dock  could  be  identified  when  the  sensor  collects  data  from  the  side.  The  goal  of  this  scan  was 
to  try  to  identify  the  whole  dock  by  collecting  data  the  whole  length  of  the  target  dock.  As  seen 
in  Figure  32,  most  of  the  dock  was  identified;  however,  there  were  a  few  pilings  missed.  It 
should  be  noticed  that  the  bottom  dock  is  not  whole  because  not  enough  sensor  scans  were 
concatenated  to  fill  the  void  of  where  the  sensor  area  under  the  LiDAR  overlapped.  The  other 
positive  aspect  from  this  scan  is  the  fact  that  parts  of  both  docks  were  identified.  Even  though 
the  two  docks  are  similar,  they  are  not  the  same  dimensions.  This  is  the  first  evidence  of  this 
algorithm  identifying  another  dock  besides  the  original  targeted  dock. 


Figure  3 1 :  Data  collected  along  the  arrow 
Imagery  ©2013  U.S.  Geological  Survey.  Map  data  ©2013  Google 


Another  issue  in  Figure  32  is  the  fact  that  there  are  a  lot  of  points  on  the  water.  These 
data  points  are  not  errors  in  the  sensor,  they  are  caused  by  debris  in  the  water,  see  Figure  33  for  a 
closer  view.  Before  this  series  of  scans  were  obtained,  the  dock  was  piled  high  with  ocean 
debris,  such  as  little  branches.  In  order  for  the  cart  to  sit  level  with  the  dock,  the  debris  was 
pushed  into  the  water.  These  pieces  of  debris  were  then  picked  up  on  the  scan.  The  relevance  of 
this  is  that  in  the  algorithm  each  data  point  creates  a  pixel  in  the  binary  images  that  is  one  square 
meter.  Now,  the  pixel  size  can  be  changed  to  represent  any  size;  however,  if  it  is  changed, 
computation  time  has  to  be  taken  into  account,  along  with  the  resizing  of  the  morphological 
structuring  elements  in  use.  A  singular  piece  of  debris  is  not  enough  to  alter  the  algorithm;  yet,  a 
few  pieces  of  debris  around  the  dock  can  cause  the  dock  to  appear  thicker  than  it  really  is  in  the 
binary  images.  This  can  lead  the  morphology  to  not  eliminate  the  dock  from  the  shore. 
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Figure  32:  Dock  identification  using  data  collected  parallel  to  target  dock 
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Figure  33:  Image  of  sensor  reading  debris  on  the  surface  of  the  water 
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All  of  the  previous  scans  were  obtained  from  the  same  dock  at  different  angles.  One  of 
the  last  scans  that  the  algorithm  was  tested  on  was  a  scan  of  a  foreign  area.  The  new  target  dock 
was  chosen  because,  while  different  from  the  original  target  dock,  it  contained  a  lot  of  the  same 
features  and  was  of  similar  size.  Figure  34  is  an  image  showing  where  the  data  was  collected 
and  the  new  target  dock,  while  Figure  35  is  an  image  of  the  target  dock. 


Figure  34:  Yellow  arrow  represents  where  the  data  was  collected  while  the  red 
star  indicates  the  target  dock 

Imagery  ©2013  U.S.  Geological  Survey.  Map  data  ©2013  Google 
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Figure  35:  Image  of  the  new  target  dock 

The  purpose  of  this  scan  was  to  test  the  algorithm’s  robustness,  meaning  that  it  could  be 
used  on  any  dock  with  similiar  dimensions  of  the  original  target  dock  as  opposed  to  only 
identifying  the  target  dock.  Figure  36  illustrates  that  the  algorithm  identified  two  docks.  The 
end  pilings  on  the  upper  dock  in  Figure  36  were  not  identified.  There  are  a  couple  possibilities 
for  why  this  happened;  the  first  is  that  the  pilings  may  be  too  tall  to  be  recognized.  These  two 
pilings  were  a  foot  and  a  half  taller  than  the  other  pilings.  Since  the  pilings  were  taller,  the 
algorithm  may  have  ignored  them  because  all  of  the  data  collected  on  known  pilings  for  the 
probability  density  function  was  gathered  off  of  shorter  pilings.  The  more  likely  reason  that 
these  pilings  were  not  identified  is  that  there  were  a  lot  of  spurious  returns  collected  underneath 
the  dock.  Unlike  the  last  scan  where  debris  caused  the  data  points,  in  this  case,  the  points  are 
caused  by  some  other  effect.  The  theory  with  this  dock  is  that  the  water  is  less  than  a  foot  deep 
and  there  are  many  rocks  under  the  dock,  so  it  is  believed  that  the  lasers  were  not  affected  by  that 
shallow  of  water.  These  spurious  returns  can  be  seen  underneath  the  dock  in  figure  37.  These 
points,  more  likely  than  not,  were  clustered  with  the  two  end  pilings.  When  the  features  were 
calculated  for  each  cluster  in  the  piling  identification  step,  these  extraneous  data  points  would 
have  altered  the  values.  This  scan  shows  that  this  algorithm  may  not  work  properly  when  the 
water  beneath  the  dock  is  shallow  enough  for  the  lasers  to  not  refract.  The  depth  of  the  water 
beneath  the  docking  locations  was  assumed  to  have  no  effect  on  the  return  of  the  scans;  however, 
this  scan  shows  evidence  that  this  cannot  be  assumed. 
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Figure  36:  Scan  of  a  new  area  with  algorithm  able  to  identify  two  docks 
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Figure  37:  This  image  illustrates  the  spurious  returns  that  were  identified  under  the  dock  during  this  scan 
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8  Conclusion  and  Future  Work 

The  previous  scans  show  that  it  is  possible  to  isolate  a  dock  from  dense  point  clouds.  The 
algorithm  works  well  when  there  are  enough  data  points  to  represent  the  whole  dock.  Since  the 
registration  process  was  long  and  arduous,  one  way  to  improve  the  testing  of  the  algorithm  is  to 
get  an  algorithm  which  uses  the  LiDAR  to  register  multiple  point  clouds  as  it  collects  them.  This 
process  would  allow  for  faster  collection  of  dense  point  clouds  and  would  be  the  first  step  to 
implementing  the  algorithm  in  real  time.  Research  is  currently  being  done  on  SLAM 
(Simultaneous  Localization  and  Mapping)  algorithms  which  do  just  that,  map  and  register 
unknown  environments.  If  an  autonomous  surface  vessel  were  to  map  an  unknown  area,  this 
algorithm  could  be  used  once  the  vessel  was  done  mapping  to  identify  docking  locations.  If  it 
were  possible  to  collect  more  dense  scans  faster,  the  algorithm  could  have  been  more  thoroughly 
tested. 


Having  more  scans  would  allow  for  more  thorough  supervised  training  of  piling  clusters 
during  the  piling  identification  step.  The  positive  identification  of  pilings  is  one  of  the  largest 
limiting  factors  in  the  algorithm  for  identifying  docks.  In  the  current  algorithm,  the  only  pilings 
included  into  the  training  data  were  pilings  from  the  target  dock.  If  known  pilings  from  other 
docks,  which  varied  in  diameter  and  height,  were  introduced  to  the  traing  data,  the  Gaussian  PDF 
would  allow  a  larger  variance  in  piling  size.  This  means  that  if  a  dock  contains  pilings  that  are 
taller,  shorter,  wider  or  thicker,  than  the  original  target  dock’s  pilings,  the  training  data  would  be 
robost  enough  to  still  identify  those  pilings. 

While  implementing  a  global  registration  process  would  be  helpful,  it  most  likely  will 
reveal  some  shortcomings  with  the  current  algorithm  that  would  need  to  be  improved.  In  theory, 
a  SLAM  algorithm  on  an  autonomus  surface  vessel  would  be  able  to  map  a  whole  harbor,  where 
there  are  multiple  docks  of  varying  shapes  and  sizes.  The  algorithm  should  be  able  to  identify 
each  dock;  however,  this  will  most  likely  not  happen.  If  a  point  cloud  map  of  a  harbor  is 
collected,  with  multiple  docks  varying  in  shape  and  size,  the  algorithm  would  then  remove  the 
shore,  and  then  attempt  to  find  the  plane.  However,  the  way  the  current  algorithm  is  written  is 
that  RANSAC  will  find  the  best  fitting  plane  from  the  point  cloud.  In  the  case  of  testing  this 
algorithm,  this  has  been  acceptable  since  the  target  docks  are  the  only  planar  surface  in  the  scan, 
and  in  each  case  with  multiple  docks,  the  docks’  planar  surfaces  have  been  the  same  height.  In 
order  to  ensure  accuracy  when  using  this  algorithm  on  multiple  docks,  each  dock  should  be  first 
clustered  and  then  have  the  planar  surface  identified  within  each  cluster.  This  will  ensure  that 
RANSAC  evaluates  the  best  fitting  plane  for  each  individual  dock  instead  of  finding  a  single 
plane  for  the  whole  point  cloud.. 

From  the  output  of  the  algorithm,  it  is  possible  to  get  specific  docking  information,  such 
as  the  centroid  of  the  dock,  location  of  individual  identified  pilings,  or  the  bounding  box 
coordinates  for  each  dock.  From  this  information,  a  program  could  be  written  to  enable  an 
autonomous  surface  vessel  to  dock  at  the  identified  docking  location.  Also,  an  algorithm  could 
be  written  to  survey  the  area  surrounding  the  dock  for  obstacles  and  this  information  could  be 
used  to  determine  the  best  docking  location  for  a  surface  vessel. 
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Overall,  the  algorithm  does  work  for  its  specific  objective;  however,  future  work  should 
initially  focus  on  expanding  the  training  data  for  pilings  while  continuing  to  test  the  algorithm  on 
docks  other  than  the  original  target  dock. 
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10  Appendices 

Appendix  A:  Parameters  Values  and  Transformation  Matrix 

Examples 


Align  point  cloud  5  (Location  310cm.pcd)  to  point  cloud  4  (Location  610cm.pcd) 
SAC-IA  Parameters  ICP  Parameters 


Number  of  Samples:  10 

Normal  Radius:  2.4 

Feature  Radius:  2.4 

Minimum  Sample  Distance:  5.0 

Maximum  Correspondence  Distance:  .001 

Maximum  of  Iterations:  500 


Max  Correspondence  Distance:  3.5 
Transformation  Epsilon:  .000005 
Maximum  of  Iterations:  200 


t 5 
‘4 


.997  -.074 

.074  .997 

-.021  -.004 

0  0 


021  -2.943- 

006  1 

1  -.2 

0  1 


Align  point  cloud  4  (Location  610cm.pcd)  to  point  cloud  3  (Location  910cm.pcd) 
SAC-IA  Parameters  ICP  Parameters 


Number  of  Samples:  10 

Normal  Radius:  2.3 

Feature  Radius:  2.3 

Minimum  Sample  Distance:  5.0 

Maximum  Correspondence  Distance:  .001 

Maximum  of  Iterations:  500 


Max  Correspondence  Distance:  4.5 
Transformation  Epsilon:  .000005 
Maximum  of  Iterations:  200 


-  .999 

.033 

-.011 

-3.126- 

-.033 

.999 

-.006 

.021 

.01 

.007 

1 

.1 

0 

0 

0 

1  J 
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Align  point  cloud  3  (Location  910cm.pcd)  to  point  cloud  2  (Location  1210cm.pcd) 
SAC-IA  Parameters  ICP  Parameters 


Number  of  Samples:  10 

Normal  Radius:  2.6 

Feature  Radius:  2.6 

Minimum  Sample  Distance:  5.0 

Maximum  Correspondence  Distance:  .001 

Maximum  of  Iterations:  500 


Max  Correspondence  Distance:  4 
Transformation  Epsilon:  .00001 
Maximum  of  Iterations:  200 


T3 

l2 


1  -.015  .015 

.015  1  -.002 

-.015  .002  1 

0  0  0 


-2.892 

.25 

.003 

1 


Align  point  cloud  2  (Location  1210cm.pcd)  to  point  cloud  1  (Location  1510cm.pcd) 
SAC-IA  Parameters  ICP  Parameters 


Number  of  Samples:  10 

Normal  Radius:  3.0 

Feature  Radius:  3.1 

Minimum  Sample  Distance:  5.0 

Maximum  Correspondence  Distance:  .001 

Maximum  of  Iterations:  500 


Max  Correspondence  Distance:  5 
Transformation  Epsilon:  .00001 
Maximum  of  Iterations:  200 


T i2 


1  .023  .004 

-.023  1  .009 

-.004  -.009  1 

0  0  0 


-3.005 

.135 

.027 

1 


Align  point  cloud  1  (Location  1510cm.pcd)  to  point  cloud  0  (Location  1810cm.pcd) 


SAC-IA  Parameters 


ICP  Parameters 


Number  of  Samples:  10 

Normal  Radius:  3.0 

Feature  Radius:  3.1 

Minimum  Sample  Distance:  5.0 

Maximum  Correspondence  Distance:  .001 

Maximum  of  Iterations:  500 


Max  Correspondence  Distance:  5 
Transformation  Epsilon:  .00001 
Maximum  of  Iterations:  200 


.999 

.032 

-.012 

-3.068-1 

-.032 

.999 

-.021 

.158 

.011 

.021 

1 

-.057 

0 

0 

0 

1  J 

Appendix  B:  Code 

Registration  Code  (Parti) 
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#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 


<iostream> 

<boost/th read/th read . hpp> 
<pcl/common/common_headers . h> 
<pcl/featunes/nonmal_3d.h> 
<pcl/io/pcd_io. h> 

< pci/ visua lizat ion/pc l_visua lizen. h> 
<pcl/console/parse.h> 
<pcl/point_types . h> 
<pcl/registration/icp. h> 

<limits> 

<fstream> 

<vecton> 

<Eigen/Cone> 

<pcl/point_cloud . h> 
<pcl/kdtree/kdtree_f lann . h> 
<pcl/filtens/passthnough .h> 
<pcl/filters/voxel_grid. h> 
<pcl/featunes/nonmal_3d. h> 
<pcl/featunes/f pf h . h> 
<pcl/registration/ia_ransac . h> 


boost: : shared_ptr<pcl : : visualization : : PCLVisualizer> 
simpleVis  (pci : : PointCloud<pcl : : PointXYZ> : :ConstPtr  cloud_in., 
pci: :PointCloud<pcl: :PointXYZ>: :ConstPtr  cloud_out) 

{ 


// 

// - Open  3D  viewer  and  add  point  cloud - 

// 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  viewer  (new 
pci : visualization :: PCLVisualizer  ("3D  Viewer"));//  creates  object  viewer 
viewer->setBackgroundColor  (0.,  0.,  0); 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_in_color(cloud_in.,  0,  0,  255);//  sets  cloud_in.,  which  will  be  cloud  out 
pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_out_color(cloud_out j  255.,  255.,  0);  //  sets  cloud_out  to  blue 

viewer->addPointCloud<pcl : : PointXYZ>  (cloud_inj  cloud_in_color.,  "cloud_in" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_in 

viewer->addPointCloud<pcl:  :PointXYZ>  (cloud_out.,  cloud_out_color.,  "cloud_out" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_out 
viewer -> set PointCloud Render ingProperties 
(pci: visualization: : PCL_VISUALIZER_POINT_SIZE,  1,  "cloud_in" ) ; 
viewer ->addCoordinateSy stem  (1.0) ; 
viewer->initCameraParameters  (); 
return  (viewer); 

> 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer> 


59 


simpleVis  (pci: : PointClouckpcl : :PointXYZ>: :ConstPtn  finals 
pci: :PointCloud<pcl: :PointXYZ>: :ConstPtn  cloud_in, 
pci: :PointCloud<pcl: :PointXYZ>: :ConstPtn  cloud_out, 
pci: :PointCloud<pcl: :PointXYZ>: :ConstPtn  icp) 

{ 

// 

// - Open  3D  viewer  and  add  point  cloud - 

// 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  viewer2  (new 
pci : visualization :: PCLVisualizer  ("3D  Viewer"));//  creates  object  viewer 
viewer2->setBackgroundColor  (0,  0,  0); 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ>  final_color(f inal,  255, 
0,  0);//  sets  final  to  red 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_in_color(cloud_in,  0,  255,  0);//  cloud  in  is  green 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_out_color(cloud_out,  0,  0,  255);  //  sets  cloud_out  to  blue 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ>  cloud_icp_color(icp, 
255,  255,  0);  //  colors  icp  yellow 

viewer2->addPointCloud<pcl: :PointXYZ>  (final,  final_color,  "final");  //adds  point  cloud 
and  defines  it  as  final 

viewer2->addPointCloud<pcl: :PointXYZ>  (cloud_in,  cloud_in_color,  "cloud_in" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_in 

viewer2->addPointCloud<pcl: :PointXYZ>  (cloud_out,  cloud_out_color,  "cloud_out" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_out 

viewer2->addPointCloud<pcl: :PointXYZ>  (icp,  cloud_icp_color,  "icp"); 
viewer 2 ->setPointCloudRenderingProperties 
(pci: visualization: :PCL_VISUALIZER_POINT_SIZE,  1,  "final"); 
viewer2->addCoordinateSystem  (1.0); 
viewer 2 - >initCameraParameters  () ; 
return  (viewer2); 

} 

class  FeatureCloud 

{ 

public : 

//  A  bit  of  shorthand 

typedef  pci : : PointCloud<pcl : : PointXYZ>  PointCloud; 
typedef  pci : : PointCloud<pcl : : Normal>  SurfaceNormals; 
typedef  pci : : PointCloud<pcl : : FPFHSignature33>  LocalFeatures; 
typedef  pci :: search :: KdTree<pcl :: PointXYZ>  SearchMethod; 

FeatureCloud  ()  : 

search_method_xyz_  (new  SearchMethod) , 
normal_radius_  (1.4f), 
feature_radius_  (1.5f) 

{} 

^FeatureCloud  ()  {} 

//  Process  the  given  cloud 
void 

setlnputCloud  (PointCloud :: Ptr  xyz) 

{ 

xyz_  =  xyz; 
processlnput  (); 

} 


//  Load  and  process  the  cloud  in  the  given  PCD  file 
void 

loadlnputCloud  (const  std:: string  &pcd_file) 

{ 

xyz_  =  PointCloud : : Ptr  (new  PointCloud); 
pci:  :io:  :loadPCDFile  (pcd_file.»  *xyz_); 
processlnput  (); 

} 

//  Get  a  pointer  to  the  cloud  3D  points 
PointCloud : : Ptr 
getPointCloud  ()  const 
{ 

return  (xyz_); 

} 

//  Get  a  pointer  to  the  cloud  of  3D  surface  normals 
SurfaceNormals : :Ptr 
getSurfaceNormals  ()  const 
{ 

return  (normals_); 

> 

//  Get  a  pointer  to  the  cloud  of  feature  descriptors 
LocalFeatures : :Ptr 
getLocalFeatures  ()  const 
{ 

return  (features_); 

} 

protected : 

//  Compute  the  surface  normals  and  local  features 
void 

processlnput  () 

{ 

computeSurfaceNormals  (); 
computeLocalFeatures  (); 

} 

//  Compute  the  surface  normals 
void 

computeSurfaceNormals  () 

{ 

normals_  =  SurfaceNormals :: Ptr  (new  SurfaceNormals) ; 

pci: :NormalEstimation<pcl: :PointXYZj  pcl::Normal>  norm_est 

norm_est . setlnputCloud  (xyz_); 

norm_est . setSearchMethod  (search_method_xyz_) ; 

norm_est . setRadiusSearch  (normal_radius_) ; 

norm_est . compute  (*normals_)j 

} 

//  Compute  the  local  feature  descriptors 
void 

computeLocalFeatures  () 

{ 

features_  =  LocalFeatures :: Ptr  (new  LocalFeatures); 


pci: :FPFHEstimation<pcl: rPointXYZ^  pci: formal,  pci: : FPFFISignature33>  fpfh_est 

fpfh_est . setlnputCloud  (xyz_); 

f pf h_est . setlnputNonmals  (normals_) ; 

fpfh_est . setSearchMethod  (search_method_xyz_) ; 

f pf h_est . setRadiusSearch  (f eatune_nadius_) ; 

fpfh_est .compute  (*features_) ; 

} 

private: 

//  Point  cloud  data 
PointCloud : : Ptr  xyz_; 

SurfaceNormals : : Ptr  normals_; 

LocalFeatures : : Ptr  features_; 

SearchMethod : :Ptr  search_method_xyz_; 

//  Parameters 

float  normal_radius_; 

float  feature_radius_; 


class  TemplateAlignment 

{ 

public : 

//  A  struct  for  storing  alignment  results 
struct  Result 
{ 

float  fitness_score; 

Eigen: :Matrix4f  f inal_transformation ; 

EIGEN_MAKE_ALIGNED_0PERAT0R_NE1aI 

}; 

TemplateAlignment  ()  : 

min_sample_distance_  (3.0f)j 
max_correspondence_distance_  ( . 001f ) ^ 
nr_iterations_  (500) 

{ 

//  Intialize  the  parameters  in  the  Sample  Consensus  Intial  Alignment  (SAC-IA) 
algorithm 

sac_ia_.  setl\lumber0fSamples(5) ; 
sac_ia_. setMinSampleDistance  (min_sample_distance_) ; 
sac_ia_. setMaxCorrespondenceDistance  (max_correspondence_distance_) ; 
sac_ia_. setMaximumlterations  (nr_iterations_) ; 

> 


^TemplateAlignment  ()  {} 

//  Set  the  given  cloud  as  the  target  to  which  the  templates  will  be  aligned 
void 

setTargetCloud  (FeatureCloud  &target_cloud) 

{ 

target_  =  target_cloud; 

sac_ia_. setlnputTarget  (target_cloud . get PointCloud  ( ) ) ; 
sac_ia_. setTarget Features  (target_cloud . get LocalFeatures  ()); 

} 
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//  Add  the  given  cloud  to  the  list  of  template  clouds 
void 

addTemplateCloud  (FeatureCloud  &template_cloud) 

{ 

templates_. push_back  (template_cloud) ; 

} 

//  Align  the  given  template  cloud  to  the  target  specified  by  setTargetCloud  () 
void 

align  (FeatureCloud  &template_cloud ,  TemplateAlignment :: Result  &result) 

{ 

sac_ia_. setlnputCloud  (template_cloud . getPointCloud  ( ) ) ; 
sac_ia_. setSourceFeatures  (template_cloud.getLocalFeatures  () ); 

pci: :PointCloud<pcl: :PointXYZ>  registration_output; 
sac_ia_. align  (registration_output) ; 

result ,fitness_score  =  (float)  sac_ia_.getFitnessScore 
(max_correspondence_distance_) ; 

result .f inal_transformation  =  sac_ia_.getFinalTransformation  (); 

} 

II  Align  all  of  template  clouds  set  by  addTemplateCloud  to  the  target  specified  by 
setTargetCloud  () 
void 

alignAll  (std : : vector<TemplateAlignment : : Result,  Eigen :: aligned_allocator<Result>  > 
&results) 

{ 

results . resize  (templates_. size  ()); 

for  (size_t  i  =  0;  i  <  templates_. size  ();  ++i) 

{ 

align  (templates_[i] j  results[i]); 

} 

} 

//  Align  all  of  template  clouds  to  the  target  cloud  to  find  the  one  with  best 
alignment  score 
int 

findBestAlignment  (TemplateAlignment :: Result  &result) 

{ 

//  Align  all  of  the  templates  to  the  target  cloud 

std : : vector<Result j  Eigen :: aligned_allocator<Result>  >  results; 

alignAll  (results); 

//  Find  the  template  with  the  best  (lowest)  fitness  score 
float  lowest_score  =  std : : numeric_limits<f loat> : : inf inity  (); 
int  best_template  =  0; 

for  (size_t  i  =  0;  i  <  results. size  ();  ++i) 

{ 

const  Result  &r  =  results[i]; 
if  (r ,fitness_score  <  lowest_score) 

{ 

lowest_score  =  r ,fitness_score; 
best_template  =  (int)  i; 

} 

} 

//  Output  the  best  alignment 
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result  =  results[best_template] ; 
return  (best_template) ; 

> 

private : 

//  A  list  of  template  clouds  and  the  target  to  which  they  will  be  aligned 
std: :vector<FeatureCloud>  templates_; 

FeatureCloud  target_; 

//  The  Sample  Consensus  Initial  Alignment  (SAC-IA)  registration  routine  and  its 
parameters 

pci: :SampleConsensusInitialAlignment<pcl : :PointXYZ,  pci: :PointXYZ, 
pci:  : FPFFISignature33>  sac_ia_; 
float  min_sample_distance_; 
float  max_correspondence_distance_; 
int  nr_iterations_; 


int 

main  (int  argc,  char**  argv) 

{ 

//If  need  to  filter  point  clouds,  un-comment  the  declarations  of  cloud_in_unf ilteredl  & 

2 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_in_unf ilteredl  (new 
pci : : PointCloud<pcl : : PointXYZ>) ;  //  cloud_in  will  hopefully  be  base  of  registration 
pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_in_unf iltered2  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_in_unf iltered3  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_out  (new  pci : : PointCloud<pcl : : PointXYZ>) ;  // 
cloud_out  will  be  registered  to  cloud 

pci: :PointCloud<pcl : :PointXYZ>: :Ptr  cloud_in  (new  pci: : PointCloud<pcl : :PointXYZ>); 


pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_out_unfilteredl  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_out_unfiltered2  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  cloud_out_unf iltered3  (new 
pci: :PointCloud<pcl: :PointXYZ>)j 


pci: :io: : loadPCDFile( "IncrementalDockData\\Line4  750cm. pcd",  *cloud_in_unf ilteredl) ; 

pci: :PassThrough<pcl: :PointXYZ>  pass; 

pass .set InputCloud  (cloud_in_unf ilteredl) ; 

pass . setFilterFieldName  ( "z" ) ; 

pass.setFilterLimits  (-5.0,  0.0); 

pass .filter (*cloud_in_unfiltered2); 

pass . set InputCloud (cloud_in_unfiltered2) ; 

pass . setFilterFieldName  ( "y" ) ; 

pass.setFilterLimits  (-30.0,  10.0); 

pass .filter (*cloud_in_unf iltered3) ; 

pass . setInputCloud(cloud_in_unf iltered3) ; 

pass. setFilterFieldName  ("x"); 

pass . set Filter Limits  ( -30.0,  10.0) ; 

pass .filter (*cloud_in); 


FeatureCloud  template_cloud; 
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template_cloud . setInputCloud(cloud_in) ; 

pci: :io: :loadPCDFile  ( "IncnementalDockData\\Line4  900cm. pcd",  *cloud_out_unf iltenedl) ; 

pass. set InputCloud  ( cloud_out_unf iltenedl ) ; 
pass . setFilterFieldName  ( "z" ) ; 
pass.setFilterLimits  (-5.0,  0.0); 
pass .f ilter(*cloud_out_unf iltered2) ; 
pass . setInputCloud(cloud_out_unfiltered2) ; 
pass. setFilterFieldName  ("y"); 
pass.setFilterLimits  (-30.0,  10.0); 
pass .f ilter(*cloud_out_unfiltered3) ; 
pass .set InputCloud (cloud_out_unfiltered3); 
pass. setFilterFieldName  ("x"); 
pass . set Filter Limits  ( -30.0,  10.0) ; 
pass .filter (*cloud_out) ; 

//  Assign  to  the  target  FeatureCloud 
FeatureCloud  target_cloud; 
target_cloud . setlnputCloud  (cloud_out) ; 

//  Set  the  TemplateAlignment  inputs 
TemplateAlignment  template_align; 

//  for  (size_t  i  =  0;  i  <  object_templates . size  ();  ++i) 

//{ 

template_align . addTemplateCloud  (template_cloud) ; 

//  } 

template_align . setTargetCloud  (target_cloud) ; 

//  Find  the  best  template  alignment 
TemplateAlignment: : Result  best_alignment; 

int  best_index  =  template_align .f indBestAlignment  (best_alignment); 
const  FeatureCloud  &best_template  =  template_cloud; 

//  Print  the  alignment  fitness  score  (values  less  than  0.00002  are  good) 
printf  ("Best  fitness  score:  %f\n",  best_alignment .f itness_score) ; 


//  Print  the  rotation  matrix  and  translation  vector 

Eigen : :Matrix3f  rotation  =  best_alignment .final_transformation . block<3, 3>(0,  0); 
Eigen : :Vector3f  translation  =  best_alignment .f inal_transformation . block<3, 1>(0,  3); 
Eigen : :Matrix4f  SACIAMatrix  =  best_alignment .f inal_transformation; 


printf  ( "\n" ) ; 
printf  ("  | 

(0,2)); 

printf  ("R  =  | 

(1,2)); 

printf  ("  | 

(2,2)); 

printf  ("\n"); 
printf  ("t  =  < 
(2)); 


%6.3f  %6.3f  %6.3f  | 
%6.3f  %6.3f  %6.3f  | 
%6.3f  %6.3f  %6.3f  | 

%0.3f,  %0. 3f j  %0.3f 


\n". 

rotation 

(0,0), 

rotation 

(0,1), 

rotation 

\n". 

rotation 

(1,0), 

rotation 

(1,1), 

rotation 

\n". 

rotation 

(2,0), 

rotation 

(2,1), 

rotation 

>\n"J  translation  (0),  translation  (1)^  translation 


//  Save  the  aligned  template  for  visualization 
pci: :PointCloud<pcl: :PointXYZ>  final; 

pci: :transformPointCloud  (*best_template.getPointCloud  (),  finals 
best_alignment .final_transformation); 

pci: :io: :savePCDFile  ("NewFilel.pcd",  final); 

pci: :PointCloud<pcl: :PointXYZ>: :ConstPtr  finalPtr  (  &final); 


pci : : IterativeClosestPoint<pcl : : PointXYZ ^  pci : : PointXYZ>  icp ; 

//icp. setRANSACOutlienRejectionThreshold( ) ; 

icp. setMaxConnespondenceDistance(3.0) ; 

icp. setTransf onmat ion Epsilon  ( . 00001) ; 

icp. setMaximumltenations  (200); 

icp. setlnputCloud(finalPtn) ; 

icp. setInputTarget(cloud_out) ; 

pci: :PointCloud<pcl: :PointXYZ>  ICPFinal; 

icp. align ( ICPF inal ) ; 

//icp. get F inalTnansf onmat ion ( )> FinalMatnix; 


Eigen : :Matnix4f  notationICP  =  icp.getFinalTnansfonmation() ; 

//  Eigen : :Vector3f  TnanslationICP  =  icp.getFinalTnansfonmation() .block<3Jl>(0J  3); 
printf("The  fitness  scone  is:  %f\n"J  icp.getFitnessSco ne()); 


pnintf  ("\n"); 

pnintf  ("  |  %6.3f  %6.3f  %6.3f  %6.3f 

notationICP  (0^2)^  notation  ICP  (0., 3) ) ; 

pnintf  ("R  =  |  %6.3f  %6.3f  %6.3f  %6.3f 
notationICP  (1,2) ,  notationICP(lJ 3) ) ; 

pnintf  ("  |  %6.3f  %6.3f  %6.3f  %6.3f 

notationICP  (2, 2) ,  notationICP(2.»  3) ) ; 

pnintf  (”  |  %6.3f  %6.3f  %6.3f  %6.3f 

notationICP  (3^2)^  notationICP(3.»  3) ) ; 
pnintf  ( "\n" ) ; 


|  \n" j  notationICP  (0,0).,  notationICP  (0,1) 

notationICP  (1,1) 
notationICP  (2,1) 
notationICP  (3,1) 


|  \n",  notationICP  (1,0) 
|  \n",  notationICP  (2,0) 
|  \n",  notationICP  (3,0) 


//Save  ICPFinal  as  PCD  File 

pci: :io: :savePCDFile  ( "NewFile2. pcd" ,  ICPFinal); 

pci: :PointCloud<pcl: :PointXYZ>: :ConstPtn  ICPFinalPtn  (  &ICPFinal); 


Eigen: :Matnix4f  FinalMatnix  =  SACIAMatnix  *  notationICP; 


pnintf ( "Final  Tnansfonmation  Matnix\n"); 
pnintf  ( "\n" ) ; 

pnintf  ("  |  %6.3f  %6.3f  %6.3f  %6.3f  | 

FinalMatnix  (0, 2) ,  FinalMatnix(0, 3) ) ; 

pnintf  ("R  =  |  %6.3f  %6.3f  %6.3f  %6.3f  | 
FinalMatnix  (1, 2) ,  FinalMatnix(l, 3) ) ; 

pnintf  ("  |  %6.3f  %6.3f  %6.3f  %6.3f  | 

FinalMatnix  {2, 2) ,  FinalMatnix(2, 3) ) ; 

pnintf  ("  |  %6.3f  %6.3f  %6.3f  %6.3f  | 

FinalMatnix  (3,2),  FinalMatnix(3, 3) ) ; 
pnintf  ( "\n" ) ; 


\n"  j 

FinalMatnix 

(0,0), 

FinalMatnix 

(0,1) 

\n", 

FinalMatnix 

(1,0), 

FinalMatnix 

(1,1) 

\n" , 

FinalMatnix 

(2,0), 

FinalMatnix 

(2,1) 

\n"  j 

FinalMatnix 

(3,0), 

FinalMatnix 

(3,1) 

boost: : shaned_ptn<pcl : visualization: : PCLVisualizen>  viewen; 
boost: : shaned_ptn<pcl : visualization: : PCLVisualizen>  viewen2; 
viewen=  simpleVis(cloud_out ,  ICPFinalPtn); 

viewen2=  simpleVis(f inalPtn,  cloud_in,  cloud_out,  ICPFinalPtn); 


while  ( ! viewen->wasStopped  ()) 

{ 

viewen->spinOnce  (100); 

boost: :this_thnead : :sleep  (boost: :posix_time: :micnoseconds  (100000)); 

} 

while  ( ! viewen2->wasStopped  ()) 

{ 


viewen2->spin0nce  (100); 
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boost: :this_thnead: :sleep  (boost: :posix_time: :micnoseconds  (100000)); 

} 

*cloud_out  +=  ICPFinal; 

pci: :io: :savePCDFile("ConcatenatedFields.pcd"J  *cloud_out); 

} 


Concatenation  Code  (Part  2) 


#include  <iostneam> 

#include  <Eigen/Dense> 

#include  <boost/thnead/thnead . hpp> 

#include  <pcl/common/common_headens . h> 

#include  <pcl/featunes/nonmal_3d. h> 

#include  <pcl/io/pcd_io. h> 

#include  < pci/ visua lizat ion/pc l_visua lizen. h> 

#include  <pcl/console/panse.h> 

#include  <pcl/point_types . h> 

#include  <pcl/registration/icp. h> 

#include  <limits> 

#include  <vector> 

#include  <Eigen/Co ne> 

#include  <pcl/point_cloud . h> 

#include  <pcl/kdtnee/kdtnee_f lann . h> 

#include  <pcl/featunes/nonmal_3d. h> 

#include  <pcl/featunes/fpfh . h> 

#include  <pcl/registration/ia_ransac . h> 

#include  <pcl/common/transforms .h> 
using  Eigen : :Matrix4f; 

boost: : shared_ptr<pcl : visualization: : PCLVisualizen>  simpleVis 
(pci: :PointCloud<pcl: :PointXYZ>: :ConstPtn  Cloudl, 


pci: : PointCloud<pcl : :PointXYZ>: :ConstPtn  Cloud2) 


{ 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  Viewer  (new 
pci : visualization :: PCLVisualizer  ("3D  Viewer")); 

Viewer->setBackgroundColor  (0^  d,  0); 

pci: visualization: : PointCloudColorFlandlerCustom<pcl : :PointXYZ>  Cloudl_Color 
(Cloudlj  0,  255 j  0); 

pci: visualization: : PointCloudColorFlandlerCustom<pcl : :PointXYZ>  Cloud2_Color 
(Cloud2,  255 j  Qj  0); 

//pci: visualization: : PointCloudColorFlandlerCustom<pcl : :PointXYZ>  Cloud3_Color 
(Cloud3j  0 j  255 j  0); 

Viewer->addPointCloud<pcl : :PointXYZ>  (Cloudl.,  Cloudl_Color ^  "Cloudl"); 
Viewer->addPointCloud<pcl : :PointXYZ>  (Cloud2j  Cloud2_Color ^  "Cloud2"); 
//Viewer->addPointCloud<pcl: :PointXYZ>  (Cloud3j  Cloud3_Color ^  "Cloud3"); 

Viewer ->addCoordinateSystem( 1.0); 
return(Viewer) ; 

} 


int 


main(int  argc.,  char**  argv) 

{ 

pci: :PointCloud<pcl: :PointXYZ>  Cloud©; 
pci: :PointCloud<pcl: :PointXYZ>  Cloudl; 
pci: :PointCloud<pcl: :PointXYZ>  Cloudla; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud2; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud2a; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud3; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud3a; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud4; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud4a; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud5; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud5a; 

pci: :io: : loadPCDFile( "IncrementalDockData\\Location 
pci:  :io:  : loadPCDFile( "IncrementalDockDataWLocation 
pci:  :io:  : loadPCDFile( "IncrementalDockDataWLocation 
pci:  :io:  : loadPCDFile( "IncrementalDockDataWLocation 
pci:  :io:  : loadPCDFile( "IncrementalDockDataWLocation 
pci:  :io:  : loadPCDFile( "IncrementalDockDataWLocation 


310cm. pcd" , 
610cm. pcd", 
910cm. pcd" , 
1210cm. pcd" 
1510cm. pcd" 
1810cm. pcd" 


Matrix4f  m54(4.,4);  //Example  of  how  to  declare  a  transformation  matrix 

m54(0,0)=.997; 

m54(lj  0)  = . 074; 

m54(2j0)=- .021; 

m54(3,0)=0; 

m54(0,l)=-.074; 

m54(lj 1)= . 997; 

m54(2,l)=-.004; 

m54(3,l)=0j 

m54(0,2)=.021; 

m54(lj  2)  = . 006; 

m54(2j  2)=1; 

m54(3j  2)=0; 

m54(0j  3)  =  -2 . 943; 

m54(lj  3)=1; 

m54(2j  3)  =  - .2; 

m54(3,3)=l; 


Matrix4f  m43(4j4);  //  Flave  to  declare  the  other  matrices  here 
Matrix4f  m32(4,4); 

Matrix4f  m21(4j4) ; 


Matrix4f  ml0(4,4); 


pci: ^ransformPointCloudCCloudSj  Cloud5a.» 
pci: :transformPointCloud(Cloud4J  Cloud4a., 
pci: :transformPointCloud(Cloud3J  Cloud3a., 
pci: :transformPointCloud(Cloud2J  Cloud2a., 
pci: :transformPointCloud(CloudlJ  Cloudla., 


m54*m43*m32*m21*ml0) ; 
m43*m32*m21*ml0) ; 
m32*m21*ml0) ; 
m21*ml0); 
ml0) ; 


Cloud©  +=Cloudla; 
Cloud©  +=Cloud2a; 
Cloud0  +=Cloud3a; 
Cloud0  +=Cloud4a; 
Cloud0  +=Cloud5a; 


Cloud5) ; 
Cloud4); 
Cloud3); 
Cloud2) 
Cloudl) 
Cloud0) 
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pci:  :io:  :savePCDFile(,,TransformAttempt.pccT ,  Cloud0); 

} 


Algorithm  Code  (Part  3) 

//  Segmentation. cpp  :  Defines  the  entry  point  for  the  console  application. 

// 


#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 


<iostream> 

<fstream> 

<pcl/ModelCoefficients .h> 
<pcl/io/pcd_io.h> 

<pcl/point_types . h> 
<pcl/sample_consensus/method_types . h> 
<pcl/sample_consensus/model_types . h> 

< pci/ segment at ion/ sac_segmentat ion . h> 
<boost/th read/th read . hpp> 
<pcl/common/common_headers . h> 

< pc 1/visua lizat ion/pc l_visualizer . h> 

< pc 1/con sole/pa r se. h> 
<pcl/filters/extract_indices .h> 

< pci/ segment at ion/ext ract_c lusters . h> 
<pcl/kdtree/kdtree. h> 

< pc 1/common/ centroid . h> 

< pci/ surf ace/conca ve_hull. h> 
<Eigen/Core> 

<Eigen/Dense> 

<Eigen/StdVector> 

< Eigen/ Eigenvalues> 

<Eigen/SVD> 

<Eigen/LU> 

<Eigen/Geometry> 

<pcl/common/eigen . h> 

< pc 1/filter s/passthrough . h> 
<pcl/common/common . h> 

<cmath> 

<math.h> 

<stdio.h> 

<cv.h> 

<core_c . h> 

<mat . hpp> 

<core.hpp> 

<opencv2\core\mat . hpp> 
<opencv2\core\core.hpp> 
<opencv2\highgui\highgui . hpp> 
<opencv2\highgui\highgui_c . h> 
<opencv2\gpu\gpumat . hpp> 
<opencv2\gpu\gpu . hpp> 
<opencv2\imgproc\imgproc . hpp> 


//using  namespace  cv; 
using  namespace  std; 
using  Eigen : :Matrix2f; 


boost: : shared_ptr<pcl : visualization: : PCLVisualizer> 
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simpleVis  (pci : : PointClouckpcl : : PointXYZ> : :ConstPtr  cloud_inJ 
pci: :PointCloud<pcl: :PointXYZ>: :ConstPtr  cloud_out) 

//  above  declares  that  four  things  will  be  present  in  the  viewer:  cloud_inj 
cloud_normalslJ  cloud_out.,  and  cloud_normals2. 

{ 


// 

// - Open  3D  viewer  and  add  point  cloud - 

// 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  viewer  (new 
pci: visualization: : PCLVisualizer  ("3D  Viewer"));//  creates  object  viewer 
viewer->setBackgroundColor  (0.,  0.,  0); 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_in_color(cloud_in.,  0,  255.,  0);//  sets  cloud_in.,  which  will  be  cloud  out 
pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_out_color(cloud_out j  255.,  0.,  0);  //  sets  cloud_out  to  blue 

viewer->addPointCloud<pcl : : PointXYZ>  (cloud_inj  cloud_in_color.,  ,,cloud_in" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_in 

viewer->addPointCloud<pcl : : PointXYZ>  (cloud_outj  cloud_out_colorj  "cloud_out" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_out 
viewer -> set PointCloud Render ingProperties 
(pci: visualization: : PCL_VISUALIZER_POINT_SIZE ,  2,  "cloud_in" ) ; 

viewer -> set PointCloud Render ingProperties 
(pci: visualization: : PCL_VISUALIZER_POINT_SIZE ,  2,  "cloud_out" ) ; 
viewer ->addCoordinateSy stem  (1.0) ; 
viewer->initCameraParameters  (); 
viewer->camera_.clip[0]  =  19.9213; 
viewer->camera_. clip[l]  =  323.021; 
viewer->camera_.focal[0]  =  -38.4332; 
viewer- >camera_. focal [1]  =  -1 . 29291; 
viewer->camera_.focal[2]  =  -3.64906; 
viewer->camera_.pos[0]  =  94.565; 
viewer->camera_.pos[l]  =  -23.85; 
viewer->camera_.pos[2]  =  71.8934; 
viewer->camera_. view[0]  =  -0.49417; 
viewer- >camera_. view[l]  =  -0.00221676; 
viewer->camera_. view[2]  =  0.869362; 
viewer->camera_.window_size[0]  =  1366; 
viewer->camera_.window_size[l]  =  706; 
viewer->camera_.window_pos[0]  =  0; 
viewer->camera_.window_pos[l]  =  0; 
viewer - >updateCamera ( ) ; 
return  (viewer); 

} 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer> 
simpleVis  (pci: :PointCloud<pcl: :PointXYZ>: :ConstPtr  cloud_in) 


{ 


Open  3D  viewer  and  add  point  cloud 


// 

// 

// 
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boost: : shared_ptr<pcl : : visualization : : PCLVisualizer>  viewer2  (new 
pci :: visualization :: PCLVisualizen  ("3D  Viewer"));//  creates  object  viewer 
viewer2->setBackgroundColor  (0,  0,  0); 

pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_in_color(cloud_in,  0,  255.,  0);//  sets  cloud_in,  which  will  be  cloud  out 
//pci: visualization: : PointCloudColorHandlerCustom<pcl : :PointXYZ> 
cloud_out_color(cloud_out,  255.,  255,  0);  //  sets  cloud_out  to  blue 

viewer2->addPointCloud<pcl : : PointXYZ>  (cloud_in,  cloud_in_color,  "cloud_in" ) ;  //adds 
point  cloud  and  defines  it  as  cloud_in 

//viewer->addPointCloud<pcl : : PointXYZ>  (cloud_out,  cloud_out_color,  "cloud_out" ) ; 
//adds  point  cloud  and  defines  it  as  cloud_out 
viewer 2 - >setPointCloudRenderingProperties 
(pci: visualization: : PCL_VISUALIZER_POINT_SIZE,  2,  "cloud_in" ) ; 
viewer2->addCoordinateSystem  (1.0); 
viewer2->initCameraParameters  () ; 
viewer 2 ->camera_. clip[0]  =  19.9213; 
viewer 2 ->camera_. clip[l]  =  323.021; 
viewer2->camera_.focal[0]  =  -38.4332; 
viewer2->camera_.focal[l]  =  -1.29291; 
viewer2->camera_.focal[2]  =  -3.64906; 
viewer2->camera_. pos[0]  =  94.565; 
viewer2->camera_. pos[l]  =  -23.85; 
viewer2->camera_. pos[2]  =  71.8934; 
viewer2->camera_. view[0]  =  -0.49417; 
viewer2->camera_. view[l]  =  -0.00221676; 
viewer2->camera_. view[2]  =  0.869362; 
viewer2->camera_.window_size[0]  =  1366; 
viewer2->camera_.window_size[l]  =  706; 
viewer2->camera_. window_pos [0]  =  0; 
viewer2->camera_.window_pos[l]  =  0; 

/*viewer2->camera_. clip[0]  =  275.235; 
viewer2->camera_. clip[l]  =  386.96; 
viewer2->camera_.focal[0]  =  -31.3541; 
viewer2->camera_.focal[l]  =  6.93915; 
viewer2->camera_.focal[2]  =  -3.87708; 
viewer 2 ->camera_. pos[0]  =  -72.4913; 
viewer2->camera_.pos[l]  =  26.9881; 
viewer 2 ->camera_. pos[2]  =  326.9881; 
viewer 2 ->camera_. view[0]  =  0.0299683; 
viewer2->camera_. view[l]  =  0.997936; 
viewer 2 ->camera_. view[2]  =  -0.0567898; 
viewer2->camera_.window_size[0]  =  1366; 
viewer2->camera_.window_size[l]  =  706; 
viewer2->camera_.window_pos[0]  =  0; 
viewer2->camera_.window_pos[l]  =  0;*/ 
viewer2->updateCamera(); 
return  (viewer2); 

} 


int 

main  (int  argc,  char**  argv) 

{ 

pci: :PointCloud<pcl: :PointXYZ>  Cloud; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud7; 
pci: :PointCloud<pcl: :PointXYZ>  Cloud8; 
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pci: :PointCloud<pcl: :PointXYZ>  CloudOutliers; 
pci: :PointCloud<pcl: :PointXYZ>  Cloudlnliers; 
pci: :PointCloud<pcl: :PointXYZ>  CloudXY; 

pci: :ModelCoefficients: :Ptr  coefficients  (new  pci : :ModelCoefficients) ; 
pci : : Pointlndices : : Ptn  inliens  (new  pci : : Pointlndices) ; 

//Uncomment  the  following  lines  to  load  different  point  clouds  into  the  algorithm 

//pci: :io: : loadPCDFile( "Location  11  Scan  1  (  Elevated) . pcd",  Cloud8); 

//pci:  :io:  :  loadPCDFile(  "Location  12  Scan  1  (Elevated) .  pcd",  Cloud7); 

//pci: :io: :loadPCDFile("TransformAttempt310-1810.pcd",  Cloud); 
pci: :io: : loadPCDFile( "TransformAttemptLine4-0-900cm. pcd" ,  Cloud); 

//pci: :io: :loadPCDFile("TransformAttemptDock2.pcd",  Cloud); 

//pci:  :io:  :loadPCDFile("Transformll-10.pcd",  Cloud); 

//pci:  :io:  : loadPCDFile( "Location  1  Scan  l.pcd",  Cloud); 

//pci:  :io:  : loadPCDFile( "Location  1  Scan  3  (Elevated) . pcd".,  Cloud); 

//  This  section  is  the  start  of  the  Shore  Removal  Stage 

//Get  cloud  dimensions  and  round  value  to  nearest  integer 

Eigen : :Vector4f  min_pt_cloud; 

Eigen : :Vector4f  max_pt_cloud; 

pci : :getMinMax3D(Cloud,  min_pt_cloud,  max_pt_cloud) ; 
signed  int  Xmax_cloud,  Ymax_cloud,  Xmin_cloud,  Ymin_cloud; 

Xmax_cloud  =  ceil(max_pt_cloud[0] ) ; 

Ymax_cloud  =  ceil(max_pt_cloud[l] ) ; 

Xmin_cloud  =  floor (min_pt_cloud [0] ) ; 

Ymin_cloud  =  f loor(min_pt_cloud [1] ) ; 

//Veiw  dimensions  of  cloud 

std: :cout<<"Xmax  "<<  Xmax_cloud<< "Xmin  "<<  Xmin_cloud<<"Ymax  "<<  Ymax_cloud<< "Xmin  "<< 
Ymin_cloud<<" "<<endl; 

//  Declare  what  each  pixel  will  represent  in  size,  i.e.  size  one  means  pixel  represents  1 
//  by  1  meter  square  of  the  point  cloud,  and  a  value  of  0.5  would  represent  a  0.5  by  0.5 
//  box 

float  Pixelsize  =  1; 

//Set  number  of  pixels  in  row  and  columns  in  image  based  on  size  of  point  cloud  and 
//  desired  resolution 
int  SizeY; 

SizeY  =  pow(Pixelsize,  -1)*( (Ymax_cloud-Ymin_cloud)+2) ; 
int  SizeX; 

SizeX  =  pow(Pixelsize,  -1)*( (Xmax_cloud-Xmin_cloud)+2) ; 

//std:  :cout«"  SizeX  is  "  <<  SizeX  <<  "  and  SizeY  is  ,,<<SizeY<<M,,<<endl; 

//  Creates  empty  image 

cv::Mat  image  =  cv: :Mat : : zeros(SizeY,  SizeX,  CV_8U); 

// 

//  This  is  the  code  that  converts  the  point  cloud  to  a  binary  image 

// 

int  PixelY; 
int  PixelX; 
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for(signed  int  y  =(Ymin_cloud  -1);  y  !=  (Ymax_cloud  +1);  y++) 

{ 

//std: :cout<<"  Y  value  is  "<<  y<<  ""<<endl; 

for(signed  int  x  =  (Xmin_cloud  -1);  x  !=(Xmax_cloud+l);  x++) 

{ 

//std: :cout<<"  X  value  is  "<<  x  <<  ""<<endl; 
for(size_t  z  =  0;  z  <  Cloud. points. size  ();  z++) 

{ 

if  (  Cloud. points[z] .x  >=  x  &&  Cloud . points[z] .x  <  x+1  && 

Cloud. points[z] ,y  >=  y  &&  Cloud . points[z] .y  <  y+1) 

{ 

PixelY  =  (pow(PixelsizeJ-l)*y)+(pow(PixelsizeJ- 

l)*(abs(Ymin_cloud)+l)) ; 

PixelX  =  ( (pow(Pixelsize., -l)*x)  +  (pow(Pixelsize^- 

l)*(abs(Xmin_cloud)+l)) ); 

//std: :cout<<"  Pixel  Y  Value  is  "<<PixelY<<  "and  Pixel  X  value 

is  "<<  PixelX<<""<<endl; 

image. at<uchan>(PixelYJ  PixelX)  =  255; 

} 

} 

} 

} 

//std:  :cout<<""<<image<<,,,,<<endl; 

//  This  section  declares  the  rest  of  the  images  that  will  be  used  in  the  algorithm  for 
//  the  various  morphological  processes 

cv::Mat  image2  =  cv: :Mat : : zeros(4*SizeYj  4*SizeX,  CV_8U) ; //Larger  grayscale  image 

cv::Mat  image3(4*SizeY.,  4*SizeX^  CV_8U) ; //Will  be  binary  image  from  image2 

cv::Mat  image3b(4*SizeYJ  4*SizeX,  CV_8U); 

cv::Mat  image4(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  image5(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  image6(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  image7(4*SizeY.,  4*SizeXj  CV_8U); 

cv::Mat  image8(4*SizeY.,  4*SizeXj  CV_8U); 

cv::Mat  image9(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  imagel0(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  imagell(4*SizeYJ  4*SizeX.,  CV_8U); 

cv::Mat  imagel2(4*SizeYj  4*SizeX,  CV_8U); 

cv::Mat  imagel4(4*SizeY.,  4*SizeX.,  CV_8U); 

cv::Mat  imagel5(SizeYj  SizeX^  CV_8U); 

cv::Mat  imagel6(4*SizeYj  4*SizeX,  CV_8U); 

cv: : namedWindow( "Binary  Image" ); //Creates  window 

cv: : imshow( "Binary  Image",  image); 

//  Makes  the  original  image  easier  to  view  by  flipping  the  matrix  and  resizing  the  image 

cv: : resize(image,  imagel6,  imagel4. size( ) ) ; 
cv: :flip(imagel6,  imagel6,0); 
cv: : namedWindow( "Original  Image"); 
cv:  :imshow("Original  Image",  imagel6); 

//cv:  :imwrite("Original_Image. jpg",  imagel6); 

//This  section  uses  contours  to  eliminate  connected  pilings  below  a  certain  size 

CvMemStorage  *storage; 

CvSeq  ^Contour  =  NULL; 
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Ipllmage  *input  =  NULL; 

double  area; 

int  FoundContours  =0; 


Ipllmage*  Copylmage  =  new  Ipllmage(image) ; 
input  =  cvClonelmage(Copylmage); 
storage  =  cvCreateMemStorage(0) ; 

CvScalar  white,  black; 
white  =  CV_RGB(255, 255, 255) ; 
black  =  CV_RGB(0,  0,  0); 
int  size  =  20; 

cvFindContours(input,  storage,  &Contour,  sizeof (CvContour) ,  CV_RETR_TREE, 
CV_CHAIN_APPROX_SIMPLE,  cv: : Point (0, 0) ) ; 

//for(  int  i  =0;  i  <contours . size( ) ;  i++) 
while(Contour) 

{ 

area  =  cvContourArea(Contour,  CV_WFIOLE_SEQ) ; 
if  (0  <=  area  &&  area  <  size) 

{ 

cvDrawContours(CopyImage,  Contour,  black,  black,  -1,  CV_FILLED,  8  ); 

} 

else 

{ 

if(0<area  &&  area<=size) 

cvDrawContours(CopyImage,  Contour,  white,  white,  -1,  CV_FILLED,  8  ); 

} 

Contour  =  Contour->h_next; 

FoundContours=  FoundContours  +1; 

} 

std: :cout<<"  Found  Contours  =  "<<  FoundContours<<endl; 

//return(FoundContours) ; 

cv::Mat  imagel3  (Copylmage); 

cv: : resize(imagel3,  imagel4,  imagel4. size( ) ) ; 

cv: :flip(imagel4,  imagel4,0); 

cv: : namedWindow( "Contours" ) ; 

cv:  :imshow("Contours",  imagel4); 

//cv: : imwrite( "Small_Pixels_Removed . jpg" ,  imagel4); 
int  element_size  =  4; 

cv: : resize(image,  image2,  image2. size( ) ) ; 
cv: :flip(image2,  image2,  0); 

//  First  dilation  process 

cv::Mat  element (2*element_size  +  1,  2*element_size+l,  CV_8U); 

cv: : dilate(image2,  image3,  element,  cv: : Point (element_size, element_size) ,  1, 

cv: : BORDER_CONSTANT  );//  dilate  the  image  once 

cv : : namedWindow( "Dilated  Image" ) ; 

cv:  :threshold(image3,  image3b,  1,  255,  cv:  :TFIRESFI_BINARY) ; 
cv: :imshow( "Dilated  Image",  image3b); 

//cv: : imwrite( "First_Dilated_Image. jpg" ,  image3b); 


//  Erosion  process 
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cv:  :erode(image3b,  image4,  element,  cv: : Point(element_size,  element_size) ,  3, 
cv: : BORDER_CONSTANT  );//  the  2  erodes  the  image  twice 
cv: :  namedl/\lindow(  "Eroded  Image"); 

cv: :threshold(image4,  image5,  1,  255,  cv: :THRESH_BINARY) ;  //  Thresholds  image2  into 
output  image3 

cv: : imshow( "Eroded  Image",  image5); 

//cv : : imwrite ( " E roded_Image . j pg" ,  image5 ) ; 

//  Second  dilation  process 

cv: :dilate(image5,  image6,  element,  cv: : Point(element_size,  element_size) ,  2, 
cv: : BORDER_CONSTANT  );  //dilate  the  image 
cvNamedl/\lindow(  "Dilated  Image  2"); 

cv: :threshold(image6,  image7,  1,  255,  cv: :THRESH_BINARY) ; 
cv: : imshow( "Dilated  Image  2",  image7); 

//cv : : imwrite ( "Second_Dilated_Image . j  pg" ,  image7 ) ; 

//  Subtraction  of  shore  from  original  image 

image8  =  image2  -  image7; 

cv: : namedWindow( "Subtracted  Image"); 

cv: :imshow("Subtracted  Image",  image8); 

//cv: : imwrite("Subtracted_Image. jpg",  image8); 

cv: : resize(image8,  imagel5,  image . size( )) ; 
cv: :flip(imagel5,  imagel5,  0); 

// 

//  Following  section  converts  binary  image  of  shore  removed  to  a  point  cloud  by 
//  extracting  all  the  points  within  its  true  values 
// 


int  Xminpixel; 
int  Yminpixel; 

pci: : PointCloud<pcl : :PointXYZ>  ShoreEliminated; 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  ShoreEliminatedPointer  (new 

pci: :PointCloud<pcl: :PointXYZ>); 

for(signed  int  y  =0;  y  !=  imagel5 . rows;  y++) 

{ 

//std: :cout<<"  Y  value  is  "<<  y<<  ""<<endl; 

for(signed  int  x  =  0;  x  !=imagel5.colsj  x++) 

{ 

if (imagel5.at<uchan>(yjX)  >  0) 

{ 

Xminpixel  =  (x  - (pow(Pixelsize, -1)*( (abs(Xmin_cloud) 
+1) ) )/pow( Pixels ize, -1) ) ; 

Yminpixel  =  (y  - (pow(Pixelsize, -1)*( (abs(Ymin_cloud) 
+1)) )/pow( Pixels ize, -1) ); 

//std: :cout<<"XminPixel  "<<  Xminpixel<<  "  Yminpixel 

"<<Yminpixel<<endl; 


//std: :cout<<"  X  value  is  "<<  x  <<  ""<<endl; 
for(size_t  z  =  0;  z  <  Cloud . points . size  ();  ++z) 
{ 


if  (  Cloud. points[z] .x  >=  Xminpixel  &&  Cloud . points[z] ,x  < 
Xminpixel+1  &&  Cloud . points [z] ,y  >=  Yminpixel  &&  Cloud . points[z] .y  <  Yminpixel+1) 

{ 

ShoreEliminatedPointer- 
>points. push_back (Cloud .points [z] ); 
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} 

} 

} 

} 

} 

ShoreEliminated  +=  *ShoreEliminatedPointer; 

//std: :cout<<"image  3"<<  image3<<  ""<<endlj 

//std : : cout«"  SizeX  is  "  <<  SizeX  <<  "  and  SizeY  is  "«SizeY«""<<endl; 

//std:  :cout«"  Pixel  Y  Value  is  "«PixelY«  "and  Pixel  X  value  is  "<<  PixelX«""«endl; 

//  Following  section  is  the  removal  of  the  plane 


//  Finds  the  plane  using  RANSAC 

pci: :SACSegmentation<pcl: :PointXYZ>  seg; 

seg. setOptimizeCoefficients (false) ; 

seg. setModelType  (pci : : SACMODEL_PLANE) ; 

seg. setMethodType  (pci: :SAC_RANSAC) ; 

seg. setMaxIterat ions (1000) ; 

seg. setDistanceThreshold  (.26); 

seg. set InputCloud (ShoreEliminated .makeSharedQ ); 

seg. segment (*inlierSj  Coefficients) ; 

pci: :ExtractIndices<pcl: :PointXYZ>  extract  (true); 

extract . set InputCloud ( ShoreE liminat ed .makeSha red ( ) ) ; 
extract . set Indices  (inliers); 
extract . setNegat ive( false ) ; 
extract .filter  (Cloudlnliers) ; 

pci :: io: : savePCDFile( "Plane  Inliersl. pcd" ,  Cloudlnliers); 

ofstream  textfile2; 

textfile2.open  ("Cloudl.txt"); 

for(size_t  i=0;  i  <  Cloud . points . size  ();  ++i) 

{ 

textfile2<<" "<<Cloud . points [i] .x<<" , "<<Cloud. points [i] .y<<"\n"<<endl; 

> 

//textfile2. close() ; 

std : :cout<<"Plane  contains  "<<  Cloudlnliers . height  *  Cloudlnliers .width<<  "  data 
points"<<std : :endl; 

//  Extract  the  points  that  were  not  included  in  the  plane 

extract . set InputCloud (Shore Eliminated .makeSha red ( ) ) ; 
extract . set Indices  (inliers); 
extract . setNegat ive(t rue ) ; 
extract .filter  (CloudOutliers) ; 

pci: :io: : savePCDFile( "Plane  Outliersl . pcd" ,  CloudOutliers); 

std: :cout<<"Outlier  point  cloud  contains  "<<  CloudOutliers . height  *  CloudOutliers .width<< 
"  data  points"<<std: :endl; 

//  Used  to  find  lines 
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std ::cerr  <<  "Output:  "  <<  Cloud .makeShaned( ) ->width  *  Cloud  .makeShanedQ ->height  <<" 
Data  Points"<<  std::endl; 

pci: iseanch: :KdTnee<pcl: :PointXYZ>: :Ptn  tree  (new  pci :: search :: KdTree<pcl :: PointXYZ>) ; 
t nee ->set!nputCloud(CloudOut liens .makeShanedQ ); 


//  Stants  the  piling  necognition  phase 

//  Below  is  the  code  fon  Euclidean  Clusten  Recognition 


std: :vecton<pcl: : PointIndices>  clusten_inliens ; 

pci: : EuclideanClustenExtnaction<pcl : :PointXYZ>  ec; 
ec . setClustenTolenance( .3); 
ec . setMinClustenSize(5) ; 
ec . setMaxClustenSize(200) ; 
ec . setSeanchMethod(tnee); 

ec . set InputCloud (CloudOut liens .makeShaned( ) ) ; 
ec .ext nact(c lust en_in liens ) ; 

pci : : PCDWniten  wniten; 


ofstneam  textfile;  //initiate  textfile 
//textfile.open  ( "ClustenData3.txt" ) ;  //open  textfile 


//  Decalnes  Covaniance  Matnix  fon  the  tnained  class  of  pilings  and  not  pilings 


pci: :PointCloud<pcl: :PointXYZ>  clusten_cloudl; 
pci: :PointCloud<pcl: :PointXYZ>  Pilings; 

Eigen : :Matnix<floatj  1,  7>  CovanianceMatnixPilings; 


CovanianceMatnixPilings (0,0) 
CovanianceMatnixPilings  (l.,0) 
CovanianceMatnixPilings (2,0) 
CovanianceMatnixPilings (3 ,0) 
CovanianceMatnixPilings  (4.,  0) 
CovanianceMatnixPilings (5 ,0) 
CovanianceMatnixPilings (6^0) 
CovanianceMatnixPilings (0^ 1) 
CovanianceMatnixPilings (1^ 1) 
CovanianceMatnixPilings (2 j 1) 
CovanianceMatnixPilings (3 j 1) 
CovanianceMatnixPilings (4^ 1) 
CovanianceMatnixPilings (5 j 1) 
CovanianceMatnixPilings (6 j 1) 
CovanianceMatnixPilings (0^  2) 
CovanianceMatnixPilings (1^  2) 
CovanianceMatnixPilings (2 j  2) 
CovanianceMatnixPilings (3 j  2) 
CovanianceMatnixPilings (4^  2) 
CovanianceMatnixPilings (5 j  2) 
CovanianceMatnixPilings (6 j  2) 
CovanianceMatnixPilings (0^  3) 
CovanianceMatnixPilings (lj  3) 


=  0.0583562492492493; 

=  -0.0190657348095124; 

=  -0.00224956680661268; 

=  -0.00535996028010200; 

=  0.000246012726655487; 

=  -0.0374002175288239; 

=  -0.000642917495801380; 
=  -0.0190657348095124; 

=  0.0462733803806834; 

=  0.00459550049951883; 

=  0.00245484674934939; 

=  0.00490618148773976; 

=  0.0476752121153300; 

=  0.0562032359241755; 

=  -0.00224956680661268; 

=  0.00459550049951883; 

=  0.0148394399958443; 

=  0.0143981502225808; 

=  0.0102616807670333; 

=  -0.0505770456513280; 

=  -0.0755976583582640; 

=  -0.00535996028010168; 

=  -0.00245484674934939; 
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CovarianceMatnixPilings(2J  3) 
CovariancelvlatrixPilings(3J  3) 
CovariancelvlatrixPilings(4J  3) 
CovarianceMatrixPilings(5.,3) 
CovarianceMatrixPilings(6.,  3) 
CovarianceMatrixPilings(0.,4) 
CovarianceMatrixPilings(l.,4) 
CovarianceMatrixPilings(2.,4) 
CovarianceMatnixPilings(3J4) 
CovarianceMatnixPilings(4J4) 
CovarianceMatrixPilings(5.,4) 
CovarianceMatnixPilings(6J4) 
CovarianceMatrixPilings(0.,  5) 
CovariancelvlatrixPilings(lJ  5) 
CovarianceMatnixPilings(2J  5) 
CovarianceMatnixPilings(3J  5) 
CovariancelvlatrixPilings(4J  5) 
CovarianceMatrixPilings(5.,  5) 
CovarianceMatrixPilings(6.,  5) 
CovarianceMatrixPilings(0.,6) 
CovarianceMatrixPilings(l.,6) 
CovariancelvlatrixPilings(2J6) 
CovariancelvlatrixPilings(3J6) 
CovarianceMatnixPilings(4J6) 
CovarianceMatrixPilings(5.,6) 
CovarianceMatrixPilings(6.,6) 


0 . 0143981502225810; 

0 . 0282440133402064; 

-8 . 16450786758868e-05; 

0.000375019873311427; 

-0.114415792459341; 

0 . 000246012726655487; 

0.00490618148773976; 

0.0102616807670333; 

-8 . 16450786758868e-05; 

0.0148495108787500; 

-0.0739541867751970; 

-0.0260239782801664; 

-0.0374002175288239; 

0.0476752121153300; 

-0.0505770456513280; 

0.000375019873311427; 

-0.0739541867751967; 

0.520320249730991; 

0.266422802577038; 

-0.000642917495801380; 

0.0562032359241755; 

-0.0755976583582640; 

-0.114415792459341; 

-0.0260239782801664; 

0.266422802577038; 

0.669959376077710; 


//std:  :cout<<  "  "  <<  CovarianceMatrixPilings<<  "\n  "<<endl; 


Eigen :  :lvlatrix<floatJ  13  7>  InverseCovarianceMatrixPilings; 


InverseCovanianceMatnixPilings(0J0) 
InverseCovariancelvlatnixPilings(lJ0) 
InverseCovanianceMatnixPilings(2J0) 
InverseCovanianceMatnixPilings(3J0) 
InvenseCovaniancelvlatnixPilings(4J0) 
InverseCovanianceMatnixPilings(5J0) 
InverseCovanianceMatnixPilings(6J0) 
InverseCovariancelvlatnixPilings(0J  1) 
InverseCovanianceMatnixPilings(lJ 1) 
InverseCovanianceMatnixPilings(2J 1) 
InvenseCovaniancelvlatnixPilings(3J  1) 
InverseCovanianceMatnixPilings(4J 1) 
InvenseCovaniancelvlatnixPilings(5J  1) 
InvenseCovaniancelvlatnixPilings(6J  1) 
InverseCovanianceMatnixPilings(0J  2) 
InvenseCovaniancelvlatnixPilings(lJ  2) 
InvenseCovaniancelvlatnixPilings(2J  2) 
InverseCovanianceMatnixPilings(3J  2) 
InvenseCovaniancelvlatnixPilings(4J  2) 
InvenseCovaniancelvlatnixPilings(5J  2) 
InverseCovanianceMatnixPilings(6J  2) 
InverseCovariancelvlatnixPilings(0J  3) 
InverseCovanianceMatnixPilings(lJ  3) 
InverseCovanianceMatnixPilings(2J  3) 
InvenseCovaniancelvlatnixPilings(3J  3) 
InverseCovanianceMatnixPilings(4J  3) 
InverseCovanianceMatnixPilings(5J  3) 
InvenseCovaniancelvlatnixPilings(6J  3) 
InverseCovanianceMatnixPilings(0J4) 


23.2061319422865; 

13.4944174578186; 

-2.03269798790532; 

-37.6312310658365; 

20.1738170017152; 

8.41761247806922; 

-10.3296196911351; 

13.4944174578182; 

153.739548919322; 

198.439467130184; 

-309.761705276175; 

-408.080467423113; 

-26.7083456988526; 

-48.6241738741668; 

-2.03269798792185; 

198.439467130085; 

3205.04482449267; 

-2060.88025143045; 

-2549.47936741061; 

-16.8209401878553; 

-99.2951610843068; 

-37.6312310658300; 

-309.761705276148; 

-2060.88025143097; 

2063.19700169003; 

1622.04826303588; 

-65.9025771793146; 

234.969639809516; 

20.1738170017294; 
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InverseCovarianceMatrixPilings(lj4) 
InverseCovarianceMatrixPilings(2.,4) 
InverseCovarianceMatrixPilings(3.,4) 
InverseCovanianceMatnixPilings(4J4) 
InverseCovarianceMatrixPilings(5.,4) 
InverseCovarianceMatrixPilings(6.,4) 
InverseCovarianceMatrixPilings(0.,  5) 
InverseCovarianceMatrixPilings(l.,  5) 
InverseCovanianceMatnixPilings(2J  5) 
InverseCovanianceMatnixPilings(3J  5) 
InverseCovarianceMatrixPilings(4.,  5) 
InverseCovarianceMatrixPilings(5.,  5) 
InverseCovarianceMatrixPilings(6.,  5) 
InverseCovarianceMatrixPilings(0.,  6) 
InverseCovanianceMatnixPilings(lJ  6) 
InverseCovanianceMatnixPilings(2J  6) 
InverseCovarianceMatrixPilings(3.,  6) 
InverseCovaniancelvlatnixPilings(4J  6) 
InverseCovarianceMatrixPilings(5.,  6) 
InverseCovarianceMatrixPilings(6.,  6) 


-408.080467423025; 

-2549.47936741051; 

1622.04826303539; 

2982.98849474081; 

178.839852850429; 

68.3383123262888; 

8.41761247806985; 

-26.7083456988467; 

-16.8209401877854; 

-65.9025771793640; 

178.839852850372; 

38.7149868350255; 

-19.3531776835518; 

-10.3296196911355; 

-48.6241738741724; 

-99.2951610844277; 

234.969639809578; 

68.3383123263913; 

-19.3531776835486; 

44.8363204793429; 


Eigen: :Matrix<float 
MeanPilings(0.,0)  = 
MeanPilings(0.,l)  = 
MeanPilings(0.,  2)  = 
MeanPilings(0.,  3)  = 
MeanPilings(0.,4)  = 
MeanPilings(0.,  5)  = 
MeanPilings(0.,6)  = 


j  lj  7>  MeanPilings; 

-3.36512702702703; 

1.60986215883237; 

0.398576687408703; 

0.720446584049003; 

0.553345847825569; 

3.03936062845996; 

3.04247673542710; 


Eigen : :Matrix<float ,  13  7>  CovarianceMatrixNotPilings;  // 
the  trained  class  of  pilings 
CovarianceMatrixNotPilings(0.,0) 

Cova  rianceMatrixNot  Pilings  (l.,0) 

Cova rianceMatrixNot Pi lings (2,0) 

Cova rianceMatrixNot Pi lings (3,0) 

Cova rianceMatrixNot Pilings (4,0) 
CovarianceMatrixNotPilings(5J0) 

Cova rianceMatrixNot Pi lings (6,0) 

Cova  rianceMat  rixNot  Pilings  (0.,  1) 

Cova rianceMatrixNot Pi lings (1^ 1) 

Cova  rianceMatrixNot  Pilings  (2.,  1) 

Cova rianceMatrixNot Pilings (3 j 1) 
CovarianceMatrixNotPilings(4J 1) 

Cova  rianceMatrixNot  Pilings  (5.,  1) 

Cova rianceMatrixNot Pilings (6 j 1) 
CovarianceMatrixNotPilings(0J  2) 

Cova rianceMatrixNot Pilings (1^  2) 

Cova rianceMatrixNot Pilings (2^  2) 

Cova  rianceMatrixNot  Pi  lings  (3.,  2) 
CovarianceMatrixNotPilings(4.»  2) 

Cova  rianceMatrixNot  Pilings  (5.,  2) 
CovarianceMatrixNotPilings(6J  2) 
CovarianceMatrixNotPilings(0.,  3) 

Cova rianceMat rixNot Pi lings (1^  3) 

Cova  rianceMatrixNot  Pi  lings  (2.,  3) 

Cova  rianceMatrixNot  Pilings  (3.,  3) 
CovarianceMatrixNotPilings(4j  3) 


5.83387605811068; 

1.07280369758292; 

-0.213675715163691; 

-0.717629117386471; 

0.0197402706516416; 

2.03280691502565; 

1.69758506659330; 

1.07280369758292; 

4.05729301065676; 

0.0869132879462066; 

-0.00117156543344151; 

0.0456354114933153; 

8.83354142155646; 

5.33983278786738; 

-0.213675715163691; 

0.0869132879462066; 

0.280809083796171; 

0.502257020522342; 

0.0348353797069033; 

-0.146846515825577; 

-0.221454553877868; 

-0.717629117386471; 

-0.00117156543344151; 

0.502257020522342; 

1.62365364335768; 

-0.0698988962946746; 


Decalres  Covariance  Matrix  for 


CovanianceMatnixNotPilings(5J  3) 
Cova rianceMatrixNot Pilings (6 j  3) 
Cova  rianceMat  rixNot  Pilings  (0.,  4) 
CovanianceMatnixNotPilings(lJ4) 
Cova  rianceMat  rixNot  Pilings  (2.,  4) 
Cova  rianceMat  rixNot  Pilings  (3  .,4) 
Cova  rianceMat  rixNot  Pi  lings  (4.,  4) 
Cova  rianceMat  rixNot  Pilings  (5  .,4) 
Cova  rianceMat  rixNot  Pi  lings  (6.,  4) 
CovarianceMatrixNotPilings(0J  5) 
CovarianceMatrixNotPilings(l.»  5) 
CovanianceMatnixNotPilings(2J  5) 
CovarianceMatrixNotPilings(3.»  5) 
CovarianceMatrixNotPilings(4.»  5) 
CovanianceMatnixNotPilings(5J  5) 
CovanianceMatnixNotPilings(6J  5) 
Cova  rianceMat  rixNot  Pilings  (0.,  6) 
Cova rianceMat rixNot Pi lings (I, 6) 
Cova rianceMatrixNot Pilings (2^  6) 
Cova rianceMatrixNot Pilings (3 j  6) 
Cova  rianceMat  rixNot  Pi  lings  (4.,  6) 
Cova  rianceMat  rixNot  Pilings  (5.,  6) 
Cova  rianceMat  rixNot  Pilings  (6.,  6) 


0.724027180240994; 

-0.948715461649621; 

0.0197402706516416; 

0.0456354114933153; 

0.0348353797069033; 

-0.0698988962946746; 

0.0442685829367980; 

-0.282989171174374; 

0.111300834494996; 

2.03280691502565; 

8.83354142155646; 

-0.146846515825577; 

0.724027180240994; 

-0.282989171174374; 

26.1648589365022; 

10.8986425007864; 

1.69758506659330; 

5.33983278786738; 

-0.221454553877868; 

-0.948715461649621; 

0.111300834494996; 

10.8986425007864; 

8.29172075686866; 


Eigen : :Matrix<float ,  13  7>  InverseCovarianceMatrixNotPilings; 


In verseCova r ianceMat rixNot Pilings ( 0,0) 
InverseCovarianceMatrixNotPilings(l,,0) 
In verseCovarianceMatrixNot Pilings (2,0) 
In verseCovarianceMatrixNot Pilings (3,0) 
InverseCovanianceMatnixNotPilings(4J0) 
In verseCovarianceMatrixNot Pilings (5^0) 
In verseCovarianceMatrixNot Pilings (6 ,0) 
InverseCovarianceMatrixNotPilings(0.,  1) 
In verseCova rianceMat rixNot Pilings (I, 1) 
In  verseCovarianceMatrixNot  Pilings  (2., 1) 
In  verseCovarianceMatrixNot  Pilings  (3., 1) 
In verseCova rianceMat rixNot Pilings (4, 1) 
In  verseCova  rianceMat  rixNot  Pilings  (5., 1) 
In  verseCova  rianceMat  rixNot  Pilings  (6., 1) 
In  verseCova  rianceMat  rixNot  Pilings  (0.,  2) 
In verseCova rianceMatrixNot Pilings (I,  2) 
In verseCova rianceMatrixNot Pilings (2 , 2) 
In verseCova rianceMatrixNot Pilings (3 ,  2) 
In verseCova rianceMat rixNot Pilings (4,  2) 
In  verseCovarianceMatrixNot  Pilings  (5.,  2) 
In  verseCova  rianceMat  rixNot  Pilings  (6.,  2) 
In  verseCova  rianceMat  rixNot  Pilings  (0.,  3) 
In verseCova rianceMatrixNot Pilings (1, 3) 
In  verseCova  rianceMat  rixNot  Pilings  (2.,  3) 
In verseCovarianceMatrixNot Pilings (3, 3) 
In  verseCova  rianceMat  rixNot  Pilings  (4.,  3) 
In  verseCova  rianceMat  rixNot  Pilings  (5.,  3) 
In verseCova rianceMatrixNot Pilings (6 j  3) 
In  verseCova  r  ianceMat  rixNot  Pilings  (0.,  4) 
InverseCovarianceMatrixNotPilings(lj4) 
InverseCovarianceMatrixNotPilings(2j4) 
InverseCovarianceMatrixNotPilings(3j4) 
InverseCovarianceMatrixNotPilings(4j4) 


0.192914571433160; 

-0.129414556216729; 

-0.00174220149071247 

0.111422779242463; 

0.205934806603797; 

0.0120106529682960; 

0.0379975846296558; 

-0.129414556216729; 

5.66348898355880; 

-4.62721055750378; 

0.257879384064185; 

-1.14143301080886; 

-0.898252968699391; 

-2.51885885036097; 

-0.00174220149071218 

-4.62721055750379; 

24.6363672689037; 

-7.86344432113909; 

-28.0734474205185; 

0.700319791958438; 

2.19486597654395; 

0.111422779242463; 

0.257879384064188; 

-7.86344432113909; 

3.69967809332743; 

10.1582517977127; 

-0.189456004912508; 

0.137071087910785; 

0.205934806603797; 

-1.14143301080887; 

-28.0734474205185; 

10.1582517977127; 

77.5620551575260; 


In verseCovanianceMatnixNot Pilings (5 ^4) 
InverseCovarianceMatrixNotPilings(6,4) 
In verseCovarianceMatrixNot Pilings (0,  5) 
In verseCova rianceMatrixNot Pilings (1,  5) 
InverseCovarianceMatrixNotPilings(2,  5) 
In verseCova rianceMatrixNot Pilings (3,  5) 
In verseCova rianceMatrixNot Pilings (4,  5) 
In verseCova rianceMatrixNot Pilings (5,  5) 
In verseCovanianceMatnixNot Pilings (6 ^  5) 
In verseCova rianceMatrixNot Pilings (0,6) 
In verseCova rianceMatrixNot Pilings (1,6) 
In verseCova rianceMatrixNot Pilings (2, 6) 
In verseCova rianceMatrixNot Pilings (3, 6) 
In verseCova rianceMatrixNot Pilings (4, 6) 
In verseCova rianceMatrixNot Pilings (5, 6) 
In verseCova rianceMatrixNot Pilings (6, 6) 


1.64155809610995; 

-2.09337837989663; 

0.0120106529682960; 

-0.898252968699392; 

0.700319791958438; 

-0.189456004912507; 

1.64155809610995; 

0.304881153981280; 

0.150268353575219; 

0.0379975846296558; 

-2.51885885036097; 

2.19486597654395; 

0.137071087910785; 

-2.09337837989664; 

0.150268353575218; 

1.63984769452969; 


Eigen : :Matrix<float,  1,  7>  MeanNotPilings; 

MeanNotPilings(0,0)  =  -1.84469046494845; 

MeanNotPilings (0, 1)  =  1.50911383574009; 

MeanNotPilings (0,  2)  =  0.576742731909785; 

MeanNotPilings (0, 3)  =  1.63000391704224; 

MeanNotPilings (0,4)  =  0.396269612773835; 

MeanNotPilings (0,  5)  =  4.51507139357617; 

MeanNotPilings (0, 6)  =  1.89252992888128; 

//  The  following  code  iterates  through  each  cluster,  adds  in  the  data  points  from  the 
//planar  surface,  and  then  uses  a  Gaussian  PDF  and  BayesJ  Theorem  to  determine  if  each 
//cluster  is  a  piling 


double  DeterminantCovarianceMatrixPilings; 
double  DeterminantCovarianceMatrixNotPilings; 


DeterminantCovarianceMatrixPilings  =  2. 362248032597543e-013; 
DeterminantCovarianceMatrixNotPilings  =  0.140828758281363; 


int  n  =0; 


Eigen: :MatrixXf  InlierCentroids(200, 3) ; 
int  j=0; 

const  double  Pi  =  4.0*atan(1.0) ; 

for  (std : : vector<pcl: :PointIndices> : : const_iterator  it  =  cluster_inliers . begin  ();  it 
! =cluster_inliers .end  ();  ++it) 

{ 

pci : : PointCloud<pcl : : PointXYZ> : : Ptr  cloud_cluster  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

for  (std: :vector<int>: :const_iterator  pit  =  it->indices . begin  ();  pit  !=  it- 
>indices.end  ();  pit++) 

c loud_c luster ->points . push_back(CloudOutliers .makeShared() ->points[*pit] ) 
cloud_cluster->width=  cloud_cluster->points . size  (); 
cloud_cluster->height  =  1; 

//Declares  objects  need  to  get  Min  and  Max  values  for  each  cluster 
Eigen : :Vector4f  min_pt; 

Eigen : :Vector4f  max_pt; 

pci : : PointCloud<pcl : : PointXYZ>  cloud_in_unf ilteredl; 
pci: :PointCloud<pcl: :PointXYZ>  cloud_in; 
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//  Gets  Min  and  Max  for  each  cluster  and  then  uses  those  boundaries  as  a 
passthrough  filter 

//  threshold  for  the  plane  inlier  cloud.  Then  adds  all  points  within  the  X-Y 
boounds  of  the  cluster  from 

//the  inlier  plane  to  the  clusters. 

pci: :getMinMax3D(*cloud_cluster,  min_pt,  max_pt); 

pci: :PassThrough<pcl: :PointXYZ>  pass; 

pass . setlnputCloud  ( Cloud In lier s . makeSha red ( ) ) ; 

pass.setFilterFieldName  ("x"); 

pass. set Filter Limits  (min_pt[0] - .02,  max_pt[0]+.02); 
pass .filter (cloud_in_unfilt eredl) ; 

pass. set InputCloud( cloud_in_unf ilteredl.makeSharedQ ); 
pass.setFilterFieldName  ("y"); 

pass. set Filter Limits  (min_pt[l] - .02,  max_pt[l]+.02); 
pass.filter(cloud_in) ; 

//Cloudlnliers .points .erase (cloud_in) ; 

*cloud_cluster  +=  cloud_in; 

//std::cout  <<  "PointCloud  representing  the  Cluster"<<  j  <<":  "  <<  cloud_cluster- 

>points.size  ()  <<  "  datapoints . "  <<  std::endl; 

//Use  above  line  to  see  data  size  of  each  cluster 

//  Compute  Centrod  and  covariance  matrix  for  each  cluster 

Eigen : :Matrix  <float,  4,  1>  CentroidVector; 

pci : : compute3DCentroid(*cloud_cluster,  CentroidVector) ; //  Computes  the  centroid 
for  each  cluster  since  command  is  embedded  in  the  for  loop 

//std::cout  <<"Centroid  for  Cluster"<<  j  <<"  is  at:\n  X:"<<  CentroidVector [0]  << 
"\n  Y:"<<  CentroidVector[l]<<  "\n  Z:"  <<CentroidVector [2]<<"  "  <<  std::endl; 

Eigen: :Matrix3f  CovarianceMatrix; 

pci : : computeCovarianceMatrixNormalized(*cloud_cluster,  CentroidVector, 
CovarianceMatrix) ; 

/*  printf  (M\nn); 

printf  ("  |  %10.9f  %10.9f  %10.9f  |  \n",  CovarianceMatrix  (0,0), 

CovarianceMatrix  (0,1),  CovarianceMatrix  (0,2)); 

printf  ( " R  =  |  %10.9f  %10.9f  %10.9f  \  \n",  CovarianceMatrix  (1,0), 

CovarianceMatrix  (1,1),  CovarianceMatrix  (1,2)); 

printf  ("  |  %10.9f  %10.9f  %10.9f  \  \n",  CovarianceMatrix  (2,0), 

CovarianceMatrix  (2,1),  CovarianceMatrix  (2,2)); 
printf  ("\n");*/ 

//Below  are  the  calculations  for  the  seven  features  for  the  Gaussian  PDF 

double  ZNstd; 
double  EigShort; 
double  EigLong; 
double  EigRatio; 
double  ZRatioEigRatio; 
double  ZRatioEigMean; 
int  N=3; 


Eigen : :Vector2f  Eigenvalues; 

Eigen : :Matrix2f  Eigenmatrix; 

Eigen : :Matrix2f  XYCovarianceMatrix  =  CovarianceMatrix. block<2, 2>(0, 0) ; 
pci: :eigen22(XYCovarianceMatrix, Eigenmatrix,  Eigenvalues); 
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ZNstd  =  2*N*sqrt(CovarianceMatrix(2J2)); 

EigShort  =  2*N*sqrt (Eigenvalues(0) ) ; 

EigLong  =  2*N*sqrt(Eigenvalues(l) ) ; 

EigRatio  =  EigShort/EigLong; 

ZRatioEigRatio  =  ZNstd/EigRatio; 

ZRatioEigMean  =  ZNstd/( (EigShort  +  EigLong)/2); 

Eigen :  :lvlatrix<floatJ  1,  7>  Testdata; 

Testdata(0j0)  =  CentroidVector [2] ; 

Testdata(0jl)  =  ZNstd; 

Testdata(0j2)  =  EigShort; 

Testdata(0.,3)  =  EigLong; 

Testdata(0j4)  =  EigRatio; 

Testdata(0^ 5)  =  ZRatioEigRatio; 

Testdata(0j6)  =  ZRatioEigMean; 

//Use  to  print  Testdata.,  Accurate  compared  to  Matlab 

//std :  : cout«"  "«  Testdata(0,0)<<"  "«  Testdata(0,l)«"  "<<Testdata(0, 2)«" 
"<<Testdata(0j 3)<<"  "<<Testdata(0,4)<<"  ,,«Testdata(0,5)«"  ,,<<Testdata(0J6)<<n \n"«endl; 

//This  section  calculates  the  PDF  values  for  pilings  and  not  pilings  of  a  given 
data  set 

double  PDFPilings; 

Eigen : :Matrix<float ,  1,  7>  PDFPilingsVariablel; 

PDFPilingsVariablel  =  (Testdata  -  MeanPilings)  ; 

Eigen : :Matrix<float ,  7,  1>  PDFPilingsVariable2; 

PDFPilingsVariable2  =  PDFPilingsVariablel. transpose(); 

Eigen: :Matrix<float,  1>  7>  PDFPilingsVariable3(lJ 7) ; 

PDFPilingsVariable3  =  PDFPilingsVariablel  *  InverseCovarianceMatrixPilings; 

Eigen : :Matrix<floatJ  1 ,  1>  PDFPilingsVariable4; 

PDFPilingsVariable4  =  PDFPilingsVariable3*  PDFPilingsVariable2; 
float  PDFPilingsVariableFinal; 

PDFPilingsVariableFinal  =  PDFPilingsVariable4(0J0) ; 

PDFPilings  =  (l/((pow((Pi*2.0), (7. 0/2.0)))  * 
pow(DeterminantCovarianceMatrixPilingsJ  0. 5) ) )*(exp( -0. 5* (PDFPilingsVariableFinal) ) ) ; 

//std: :cout<<  "PDFVariable  "  <<  PDFPilings  <<  "  "  <<std::endl;  //Use  this  to  print 
PDFPilings 

double  PDFNotPilings; 

Eigen : :Matrix<float ,  1,  7>  PDFNotPilingsVariablel; 

PDFNotPilingsVariablel  =  (Testdata  -  MeanNotPilings)  ; 

Eigen : :Matrix<float ,  7,  1>  PDFNotPilingsVariable2; 

PDFNotPilingsVariable2  =  PDFNotPilingsVariablel. transpose(); 

Eigen : :Matrix<floatJ  1>  7>  PDFNotPilingsVariable3(lJ 7) ; 

PDFNotPilingsVariable3  =  PDFNotPilingsVariablel  * 

In verseCovarianceMatrixNot Pilings; 

Eigen : :Matrix<floatJ  1 ,  1>  PDFNotPilingsVariable4; 

PDFNotPilingsVariable4  =  PDFNotPilingsVariable3*  PDFNotPilingsVariable2; 
float  PDFNotPilingsVariableFinal; 

PDFNotPilingsVariableFinal  =  PDFNotPilingsVariable4(0J0) ; 

PDFNotPilings  =  (l/((pow((Pi*2.0),(7. 0/2.0)))  * 
pow( DeterminantCova rianceMatrixNot Pilings j  0. 5) ) )*(exp( - 
0. 5* (PDFNotPilingsVariableFinal) ) ) ; 
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//std: :cout<<  "PDFNotPilingsVariable  "  <<  PDFNotPilings  <<  "  "  <<std : : endl; //Use 

this  to  print  PDFNotPilings 

//std: :cout<<"EigenValues  are  "  <<Eigenvalues[0]<<  ", 

"<<Eigenvalues[l]<<std: :endl; 

//std:  :cout<<  "  ZNstd  is  "«  ZNstd  «",  EigX  is  "«  EigX<<  ",  EigY  is  "«  EigY  << 
",  Ratio  is  "  <<  EigX/EigY«"  for  cloud  "  <<  j  «std::endl; 

/*textfile  <<""<< 

CentroidVector[0]<<" , "<<CentroidVector[l]<<" , "<<CentroidVector[2]<<" ,  "  //writes  each 
cluster  data  to  a  text  file 

ccCovarianceMatrix^Ojcc'V'ccCovarianceMatrix^lJcc'V'ccCovarianceMatrix^^Jcc" 

ii 


^CovarianceMatrixCl^cc'V^CovarianceMatrixtljl^'V^CovarianceMatrixCl^cc" 

ii 


ccCovarianceMatrix^O^'V'c^ovarianceMatrix^lJcc'V'ccCovarianceMatrix^^^c" 
\n"<<endl; */ 

//std::  stringstream  ss; //Uncomment  next  three  lines  to  save  each  individual 

cluster 

//ss«  "cloud_cluster_"  <<  j  <<  " .pcd"; 

//writer .write<pcl: :PointXYZ>  (ss.strQ,  *cloud_cluster ,  false); 

//CALCULATE  PROBABILITY  OF  BEING  A  PILING 

double  ProbabilityX; 

double  ProbabilityXl; 

double  ProbabilityX2; 

double  ProbabilityPilings; 

double  ProbabilityNotPilings; 

double  Probabilityl; 

double  Probability2; 

ProbabilityPilings=  . 16296296296; 

ProbabilityNotPilings  =.83703703704; 

ProbabilityXl  =  ProbabilityPilings*PDFPilings; 

//std: :cout<<  "ProbabilityXl  =  "  <<  ProbabilityXl  <<  "  "  «std::endl; 

ProbabilityX2  =  ProbabilityNotPilings* PDFNotPilings; 

//std: :cout<<  "ProbabilityX2  =  "  <<  ProbabilityX2  <<  "  "  «std::endl; 

ProbabilityX  =  ProbabilityXl  +  ProbabilityX2; 

//std: :cout<<  "ProbabilityX  =  "  <<  ProbabilityX  <<  "  "  <<std::endl; 


Probabilityl  =  (ProbabilityPilings*PDFPilings) ; 

Probability2  =  Probabilityl/ProbabilityX; 

//std: :cout<<  ""<<Probabilityl<<"  /  "  <<  ProbabilityX  <<"  = 

"<<  Probability  2<<"  "«endl; 

//  Below  is  the  thresholding  based  off  probability  as  an  output  of  Bayes-*  Theorem 
if (Probability2  >  .5) 

{ 


InlierCentroids . row(n)  <<  CentroidVector (0) ,  CentroidVector(l) , 
CentroidVector (2) ; 
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Pilings  +=  *cloud_cluster; 
n  =  n+1; 

> 

cluster_cloudl  +=  *cloud_cluster; 

j++; 

> 

//  Below  is  the  code  that  compares  identified  pilings  with  objects  removed  from  shore 

//Convert  piling  point  cloud  to  binary  image 

cv::Mat  Pilinglmage  =  cv: :Mat : : zeros(SizeY,  SizeX,  CV_8U);; 
for(signed  int  y  =(Ymin_cloud  -1);  y  !=  (Ymax_cloud  +1);  y++) 

{ 

//std: :cout<<"  Y  value  is  "<<  y<<  ""<<endl; 

for(signed  int  x  =  (Xmin_cloud  -1);  x  !=(Xmax_cloud+l);  x++) 

{ 

//std: :cout<<"  X  value  is  "<<  x  <<  ""<<endl; 
for(size_t  z  =  0;  z  !=n  ;  z++) 

{ 

if  (  InlierCentroids(z,0)  >=  x  &&  InlierCentroids(z,0)  <  x+1  && 
InlierCentroids(z,l)  >=  y  &&  InlierCentroids(z, 1)  <  y+1) 

{ 

PixelY  =  (pow(Pixelsize,-l)*y)+(pow(Pixelsize,- 

l)*(abs(Ymin_cloud)+l) ) ; 

PixelX  =  ( (pow(Pixelsize, -l)*x)  +  (pow(Pixelsize,- 

l)*(abs(Xmin_cloud)+l) ) ) ; 

//std: :cout<<"  Pixel  Y  Value  is  "<<PixelY<<  "and  Pixel  X  value 

is  "<<  PixelX<<""<<endl; 

PixelGrid(PixelY,  PixelX)  =1; 

Pilinglmage. at<uchar>(PixelYj  PixelX)  =  255; 

} 


} 

} 

} 

cv::Mat  Pilinglmage2  =  cv: :Mat : :zeros(4*SizeY,  4*SizeX,  CV_8U); 

cv: : resize(PilingImage,  Pilinglmage2,  Pilinglmage2.size()); 

cv: :flip(PilingImage2,  Pilinglmage2,  0); 

cv: : namedWindow( "Piling  Image"); 

cv: : imshow( "Piling  Image",  Pilinglmage2) ; 

//cv: : imwrite("Piling_Image. jpg",  Pilinglmage2) ; 

//  Erosion  and  dilation  process 

int  element_size2  =  7; 
int  element_size3  =  3; 

cv::Mat  element2(2*element_size2  +  1,  2*element_size2  +  1,  CV_8U); 
cv::Mat  element3(2*element_size3  +  1,  2*element_size3  +  1,  CV_8U); 

cv: :dilate(PilingImage2,  imagel0,  element2,  cv :: Point (element_size2, element_size2) ,  1, 
cv : : BORDER_CONSTANT  ) ; 

cv: :erode(imagel0,  imagel0,  element3,  cv: :Point(element_size3,element_size3),  1, 
cv: :BORDER_CONSTANT  ); 

cv: :threshold(imagel0,  imagel0,  1,  255,  cv: :THRESH_BINARY) ; 


cv: : namedWindow( "Dilated  Pilings"); 
cv: : imshow( "Dilated  Pilings",  imagel0); 

//cv: : imwrite( "Dilated_Pilings . jpg" ,  imagel0); 

//  Multiply  pilings  by  objects  removed  from  shore  to  verify  that  both  are  true 

cv: :multiply(image8  ,  imagel0,  image9,  1); 

cv: : namedl/\lindow( "Pilings  Multiplied  with  Morphology"); 

cv: : imshow( "Pilings  Multiplied  with  Morphology",  image9); 

cv: : resize(image9,  imagell,  image. sizeQ); 
cv: :flip(imagell,  imagell,  0); 
cv : : namedWindow( "Dock" ) ; 
cv: : imshow( "Dock" ,  imagell); 

int  Xminpixel2; 

//int  Xmaxpixel; 
int  Yminpixel2; 

//int  Ymaxpixel; 

pci : : PointCloud<pcl : : PointXYZ>  Identif iedDock; 

pci: :PointCloud<pcl: :PointXYZ>: :Ptr  Identif iedDockPointer  (new 

pci: :PointCloud<pcl: :PointXYZ>); 

pci: :PointXYZ  Position; 

size_t  counter  =  1; 

//  Convert  binary  image  to  point  cloud  based  on  binary  image-* s  true  pixel  values 

for(signed  int  y  =0;  y  !=  imagell . rows;  y++) 

{ 

//std: :cout<<"  Y  value  is  "<<  y<<  ""<<endl; 

for(signed  int  x  =  0;  x  !=imagell.cols;  x++) 

{ 

if  (imagell. at<uchar>(y.,x)  >  0) 

{ 

Xminpixel2  =  (x  - (pow(Pixelsize, -1)*( (abs(Xmin_cloud) 

+1)) )/pow(Pixelsize, -1) ); 

Yminpixel2=  (y  - (pow(Pixelsize, -1)*( (abs(Ymin_cloud) 

+1)) )/pow(Pixelsize, -1) ); 

//std: :cout<<"XminPixel  "<<  Xminpixel<<  "  Yminpixel 

"<<Yminpixel<<endl; 

//std: :cout<<"  X  value  is  "<<  x  <<  ""<<endl; 
for(size_t  z  =  0;  z  <  Cloud . points . size  ();  ++z) 

{ 


if  (  Cloud. points[z] .x  >=  Xminpixel2  &&  Cloud. points[z] .x 
Xminpixel2+1  &&  Cloud . points[z] .y  >=  Yminpixel2  &&  Cloud . points[z] .y  <  Yminpixel2+1) 

{ 

IdentifiedDockPointer- 

>points . pu sh_back( Cloud .points [z] ) ; 

} 

> 

> 

> 

> 

IdentifiedDock  +=  *IdentifiedDockPointer; 
std: :vector<pcl: : PointIndices>  cluster_inliers2; 
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//  Cluster  objects  from  output  point  cloud 

ec . setClusterTolerance(l) ; 
ec . setMinClusterSize( 5) ; 
ec . setMaxClusterSize( 100000) ; 
ec . setSearchMet hod (tree) ; 

ec . setInputCloud(IdentifiedDock.makeShared()) ; 
ec .ext ract(c lust er_inliers2); 

//Determine  the  eigenvalues  for  each  cluster  and  threshold  accordingly 
pci: :PointCloud<pcl: :PointXYZ>  Identif iedDockFinal; 

for  (std : : vector<pcl: :PointIndices> : : const_iterator  it  =  cluster_inliers2.begin  ();  it 
!=cluster_inliers2.end  ();  ++it) 

{ 

pci : : PointCloud<pcl : : PointXYZ> : : Ptr  cloud_cluster2  (new 
pci: :PointCloud<pcl: :PointXYZ>); 

for  (std : : vector<int> : : const_iterator  pit  =  it->indices . begin  ();  pit  !=  it- 
>indices.end  ();  pit++) 

cloud_cluster2->points.push_back(IdentifiedDock.makeShared() - 
>points[*pit] ) ; 

{ 


Eigen : :Matrix  <float,  4}  1>  CentroidVector ; 

pci: : compute3DCentroid(*cloud_cluster2J  CentroidVector) ; 

//std::cout  <<"Centroid  for  Cluster"<<  j  <<"  is  at:\n  X:"<< 
CentroidVector[0]  <<  "\n  Y:"<<  CentroidVector[l] <<  "\n  Z:"  <<CentroidVector[2]<<"  "  << 
std : :endl; 

Eigen: :Matrix3f  CovarianceMatrix; 

pci : :  computeCovariancelvlatrixNormalized(*cloud_cluster2J  CentroidVector ^ 
CovarianceMatrix) ; 

Eigen: :Vector2f  Eigenvalues; 

Eigen :: Mat rix2f  Eigenmatrix; 

Eigen : :Matrix2f  XYCovarianceMatrix  =  CovarianceMatrix. block<2, 2>(0^0) ; 

pci: : eigen22(XYCovarianceMatriXj Eigenmatrix^  Eigenvalues); 

double  EigRatio; 

double  EigShort; 

double  EigLong; 

int  N=3; 

EigShort  =  2*N*sqrt (Eigenvalues(0) ) ; 

EigLong  =  2*N*sqrt(Eigenvalues(l) ) ; 

EigRatio  =  EigLong/EigShort; 

std : : cout<<"EigRatio  =  "  <<  EigRatio<<"  EigShort  =  "<<EigShort<<endl; 

if  (EigRatio  2  &&  EigShort>2.9  ) 

{ 

IdentifiedDockFinal  +=  *cloud_cluster2; 

} 


} 

} 

pci: :io: : savePCDFile( "Plane  Outliersl . pcd" ^  CloudOutliers) ; 

std : :cout<<"There  are  "<<  counter<<"  datapoints  within  the  bounds"<<endl; 

//std: :cout<<  InlierCentroids<<std : :endl; 

std::cout<<  "There  are  "<<  j  <<  "  clusters  that  match  the  parameters\n"<<std : : endl; 
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std: :cout<<"There  are  "  <<  n  <<  "  identified  pilings"<<endl; 
//std :  : cout<<  "Pi  =  "<<  Pi  <<  "\n  "  «std::endl; 
textfile.close() ; 

//return  0; 


//  Below  controls  the  point  cloud  viewer 

pci: :io: : savePCDFile( "Clustersl . pcd" ,  cluster_cloudl) ; 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  viewer; 

boost: : shared_ptr<pcl : visualization: : PCLVisualizer>  viewer2; 

viewer=  simpleVis (Cloud .makeShared( ) ,  IdentifiedDockFinal.makeShared( )) ; 

while  ( ! viewer->wasStopped  ()) 

{ 

viewen->spinOnce  (100); 

boost: :this_thread : : sleep  (boost: :posix_time: : microseconds 

(100000) ); 

} 

viewer2=  s impleVis( Cloud. makeSha red () ); 
while  ( ! viewer2->wasStopped  ()) 

{ 

viewer2->spin0nce  (100); 

boost: :this_thread : : sleep  (boost: :posix_time: : microseconds 

(100000)); 

} 


} 


